/*************************************************************************
* *
* 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 "pu.h"
#include "joint_internal.h"
//****************************************************************************
// Prismatic and Universal
dxJointPU::dxJointPU( dxWorld *w ) :
dxJointUniversal( w )
{
// Default Position
// Y ^ Axis2
// ^ |
// / | ^ Axis1
// Z^ / | /
// | / Body 2 | / Body 1
// | / +---------+ | / +-----------+
// | / / /| | / / /|
// | / / / + _/ - / / +
// | / / /-/--------(_)----|--- /-----------/-------> AxisP
// | / +---------+ / - +-----------+ /
// | / | |/ | |/
// | / +---------+ +-----------+
// |/
// .-----------------------------------------> X
// |----------------->
// Anchor2 <--------------|
// Anchor1
//
// Setting member variables which are w.r.t body2
dSetZero( axis1, 4 );
axis1[1] = 1;
// Setting member variables which are w.r.t body2
dSetZero( anchor2, 4 );
dSetZero( axis2, 4 );
axis2[2] = 1;
dSetZero( axisP1, 4 );
axisP1[0] = 1;
dSetZero( qrel1, 4 );
dSetZero( qrel2, 4 );
limotP.init( world );
limot1.init( world );
limot2.init( world );
}
dReal dJointGetPUPosition( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
dVector3 q;
// get the offset in global coordinates
dMultiply0_331( q, joint->node[0].body->posr.R, joint->anchor1 );
if ( joint->node[1].body )
{
dVector3 anchor2;
// get the anchor2 in global coordinates
dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
}
else
{
//N.B. When there is no body 2 the joint->anchor2 is already in
// global coordinates
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->anchor2[2] ) );
if ( joint->flags & dJOINT_REVERSE )
{
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
}
}
dVector3 axP;
// get prismatic axis in global coordinates
dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );
return dCalcVectorDot3( axP, q );
}
dReal dJointGetPUPositionRate( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->node[0].body )
{
// We want to find the rate of change of the prismatic part of the joint
// We can find it by looking at the speed difference between body1 and the
// anchor point.
// r will be used to find the distance between body1 and the anchor point
dVector3 r;
dVector3 anchor2 = {0,0,0};
if ( joint->node[1].body )
{
// Find joint->anchor2 in global coordinates
dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
r[0] = ( joint->node[0].body->posr.pos[0] -
( anchor2[0] + joint->node[1].body->posr.pos[0] ) );
r[1] = ( joint->node[0].body->posr.pos[1] -
( anchor2[1] + joint->node[1].body->posr.pos[1] ) );
r[2] = ( joint->node[0].body->posr.pos[2] -
( anchor2[2] + joint->node[1].body->posr.pos[2] ) );
}
else
{
//N.B. When there is no body 2 the joint->anchor2 is already in
// global coordinates
// r = joint->node[0].body->posr.pos - joint->anchor2;
dSubtractVectors3( r, joint->node[0].body->posr.pos, joint->anchor2 );
}
// The body1 can have velocity coming from the rotation of
// the rotoide axis. We need to remove this.
// N.B. We do vel = r X w instead of vel = w x r to have vel negative
// since we want to remove it from the linear velocity of the body
dVector3 lvel1;
dCalcVectorCross3( lvel1, r, joint->node[0].body->avel );
// lvel1 += joint->node[0].body->lvel;
dAddVectors3( lvel1, lvel1, joint->node[0].body->lvel );
// Since we want rate of change along the prismatic axis
// get axisP1 in global coordinates and get the component
// along this axis only
dVector3 axP1;
dMultiply0_331( axP1, joint->node[0].body->posr.R, joint->axisP1 );
if ( joint->node[1].body )
{
// Find the contribution of the angular rotation to the linear speed
// N.B. We do vel = r X w instead of vel = w x r to have vel negative
// since we want to remove it from the linear velocity of the body
dVector3 lvel2;
dCalcVectorCross3( lvel2, anchor2, joint->node[1].body->avel );
// lvel1 -= lvel2 + joint->node[1].body->lvel;
dVector3 tmp;
dAddVectors3( tmp, lvel2, joint->node[1].body->lvel );
dSubtractVectors3( lvel1, lvel1, tmp );
return dCalcVectorDot3( axP1, lvel1 );
}
else
{
dReal rate = dCalcVectorDot3( axP1, lvel1 );
return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
}
}
return 0.0;
}
void
dxJointPU::getSureMaxInfo( SureMaxInfo* info )
{
info->max_m = 6;
}
void
dxJointPU::getInfo1( dxJoint::Info1 *info )
{
info->m = 3;
info->nub = 3;
// powered needs an extra constraint row
// see if we're at a joint limit.
limotP.limit = 0;
if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
limotP.lostop <= limotP.histop )
{
// measure joint position
dReal pos = dJointGetPUPosition( this );
limotP.testRotationalLimit( pos ); // N.B. The function is ill named
}
if ( limotP.limit || limotP.fmax > 0 ) info->m++;
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
dxJointPU::getInfo2( dReal worldFPS, dReal worldERP,
int rowskip, dReal *J1, dReal *J2,
int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
int *findex )
{
const dReal k = worldFPS * worldERP;
// ======================================================================
// The angular constraint
//
dVector3 ax1, ax2; // Global axes of rotation
getAxis(this, ax1, axis1);
getAxis2(this,ax2, axis2);
dVector3 uniPerp; // Axis perpendicular to axes of rotation
dCalcVectorCross3(uniPerp,ax1,ax2);
dNormalize3( uniPerp );
dCopyVector3( J1 + GI2__JA_MIN, uniPerp );
dxBody *body1 = node[1].body;
if ( body1 ) {
dCopyNegatedVector3( J2 + GI2__JA_MIN , uniPerp );
}
// Corrective velocity attempting to keep uni axes perpendicular
dReal val = dCalcVectorDot3( ax1, ax2 );
// Small angle approximation :
// theta = asin(val)
// theta is approximately val when val is near zero.
pairRhsCfm[GI2_RHS] = -k * val;
// ==========================================================================
// Handle axes orthogonal to the prismatic
dVector3 an1, an2; // Global anchor positions
dVector3 axP, sep; // Prismatic axis and separation vector
getAnchor(this, an1, anchor1);
getAnchor2(this, an2, anchor2);
if (flags & dJOINT_REVERSE) {
getAxis2(this, axP, axisP1);
} else {
getAxis(this, axP, axisP1);
}
dSubtractVectors3(sep, an2, an1);
dVector3 p, q;
dPlaneSpace(axP, p, q);
dCopyVector3( J1 + rowskip + GI2__JL_MIN, p );
dCopyVector3( J1 + 2 * rowskip + GI2__JL_MIN, q );
// Make the anchors be body local
// Aliasing isn't a problem here.
dSubtractVectors3(an1, an1, node[0].body->posr.pos);
dCalcVectorCross3( J1 + rowskip + GI2__JA_MIN, an1, p );
dCalcVectorCross3( J1 + 2 * rowskip + GI2__JA_MIN, an1, q );
if (body1) {
dCopyNegatedVector3( J2 + rowskip + GI2__JL_MIN, p );
dCopyNegatedVector3( J2 + 2 * rowskip + GI2__JL_MIN, q );
dSubtractVectors3(an2, an2, body1->posr.pos);
dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, p, an2 );
dCalcVectorCross3( J2 + 2 * rowskip + GI2__JA_MIN, q, an2 );
}
pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( p, sep );
pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( q, sep );
// ==========================================================================
// Handle the limits/motors
int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip;
if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) {
currRowSkip += rowskip; currPairSkip += pairskip;
}
if (limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 )) {
currRowSkip += rowskip; currPairSkip += pairskip;
}
if ( body1 || (flags & dJOINT_REVERSE) == 0 ) {
limotP.addTwoPointLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 );
} else {
dNegateVector3(axP);
limotP.addTwoPointLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 );
}
}
void dJointSetPUAnchor( dJointID j, dReal x, dReal y, dReal z )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
joint->computeInitialRelativeRotations();
}
/**
* This function initialize the anchor and the relative position of each body
* as if body2 was at its current position + [dx,dy,dy].
* Ex:
* <PRE>
* dReal offset = 1;
* dVector3 dir;
* dJointGetPUAxis3(jId, dir);
* dJointSetPUAnchor(jId, 0, 0, 0);
* // If you request the position you will have: dJointGetPUPosition(jId) == 0
* dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset);
* // If you request the position you will have: dJointGetPUPosition(jId) == -offset
* </PRE>
* @param j The PU joint for which the anchor point will be set
* @param x The X position of the anchor point in world frame
* @param y The Y position of the anchor point in world frame
* @param z The Z position of the anchor point in world frame
* @param dx A delta to be added to the X position as if the anchor was set
* when body1 was at current_position[X] + dx
* @param dx A delta to be added to the Y position as if the anchor was set
* when body1 was at current_position[Y] + dy
* @param dx A delta to be added to the Z position as if the anchor was set
* when body1 was at current_position[Z] + dz
* @note Should have the same meaning as dJointSetSliderAxisDelta
*/
void dJointSetPUAnchorDelta( dJointID j, dReal x, dReal y, dReal z,
dReal dx, dReal dy, dReal dz )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->node[0].body )
{
joint->node[0].body->posr.pos[0] += dx;
joint->node[0].body->posr.pos[1] += dy;
joint->node[0].body->posr.pos[2] += dz;
}
setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
if ( joint->node[0].body )
{
joint->node[0].body->posr.pos[0] -= dx;
joint->node[0].body->posr.pos[1] -= dy;
joint->node[0].body->posr.pos[2] -= dz;
}
joint->computeInitialRelativeRotations();
}
/**
* \brief This function initialize the anchor and the relative position of each body
* such that dJointGetPUPosition will return the dot product of axis and [dx,dy,dy].
*
* The body 1 is moved to [-dx, -dy, -dx] then the anchor is set. This will be the
* position 0 for the prismatic part of the joint. Then the body 1 is moved to its
* original position.
*
* Ex:
* <PRE>
* dReal offset = 1;
* dVector3 dir;
* dJointGetPUAxis3(jId, dir);
* dJointSetPUAnchor(jId, 0, 0, 0);
* // If you request the position you will have: dJointGetPUPosition(jId) == 0
* dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset);
* // If you request the position you will have: dJointGetPUPosition(jId) == offset
* </PRE>
* @param j The PU joint for which the anchor point will be set
* @param x The X position of the anchor point in world frame
* @param y The Y position of the anchor point in world frame
* @param z The Z position of the anchor point in world frame
* @param dx A delta to be added to the X position as if the anchor was set
* when body1 was at current_position[X] + dx
* @param dx A delta to be added to the Y position as if the anchor was set
* when body1 was at current_position[Y] + dy
* @param dx A delta to be added to the Z position as if the anchor was set
* when body1 was at current_position[Z] + dz
* @note Should have the same meaning as dJointSetSliderAxisDelta
*/
void dJointSetPUAnchorOffset( dJointID j, dReal x, dReal y, dReal z,
dReal dx, dReal dy, dReal dz )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if (joint->flags & dJOINT_REVERSE)
{
dx = -dx;
dy = -dy;
dz = -dz;
}
if ( joint->node[0].body )
{
joint->node[0].body->posr.pos[0] -= dx;
joint->node[0].body->posr.pos[1] -= dy;
joint->node[0].body->posr.pos[2] -= dz;
}
setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
if ( joint->node[0].body )
{
joint->node[0].body->posr.pos[0] += dx;
joint->node[0].body->posr.pos[1] += dy;
joint->node[0].body->posr.pos[2] += dz;
}
joint->computeInitialRelativeRotations();
}
void dJointSetPUAxis1( dJointID j, dReal x, dReal y, dReal z )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
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 dJointSetPUAxis2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
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 dJointSetPUAxisP( dJointID id, dReal x, dReal y, dReal z )
{
dJointSetPUAxis3( id, x, y, z );
}
void dJointSetPUAxis3( dJointID j, dReal x, dReal y, dReal z )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
setAxes( joint, x, y, z, joint->axisP1, 0 );
joint->computeInitialRelativeRotations();
}
void dJointGetPUAngles( dJointID j, dReal *angle1, dReal *angle2 )
{
dxJointUniversal* joint = ( dxJointUniversal* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
joint->getAngles( angle2, angle1 );
else
joint->getAngles( angle1, angle2 );
}
dReal dJointGetPUAngle1( dJointID j )
{
dxJointUniversal* joint = ( dxJointUniversal* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
return joint->getAngle2();
else
return joint->getAngle1();
}
dReal dJointGetPUAngle2( dJointID j )
{
dxJointUniversal* joint = ( dxJointUniversal* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
return joint->getAngle1();
else
return joint->getAngle2();
}
dReal dJointGetPUAngle1Rate( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
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 dJointGetPUAngle2Rate( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
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 dJointSetPUParam( dJointID j, int parameter, dReal value )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
switch ( parameter & 0xff00 )
{
case dParamGroup1:
joint->limot1.set( parameter, value );
break;
case dParamGroup2:
joint->limot2.set( parameter & 0xff, value );
break;
case dParamGroup3:
joint->limotP.set( parameter & 0xff, value );
break;
}
}
void dJointGetPUAnchor( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
if ( joint->node[1].body )
getAnchor2( joint, result, joint->anchor2 );
else
{
// result[i] = joint->anchor2[i];
dCopyVector3( result, joint->anchor2 );
}
}
void dJointGetPUAxis1( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
getAxis2( joint, result, joint->axis2 );
else
getAxis( joint, result, joint->axis1 );
}
void dJointGetPUAxis2( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
getAxis( joint, result, joint->axis1 );
else
getAxis2( joint, result, joint->axis2 );
}
/**
* @brief Get the prismatic axis
* @ingroup joints
*
* @note This function was added for convenience it is the same as
* dJointGetPUAxis3
*/
void dJointGetPUAxisP( dJointID id, dVector3 result )
{
dJointGetPUAxis3( id, result );
}
void dJointGetPUAxis3( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
getAxis( joint, result, joint->axisP1 );
}
dReal dJointGetPUParam( dJointID j, int parameter )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
switch ( parameter & 0xff00 )
{
case dParamGroup1:
return joint->limot1.get( parameter );
break;
case dParamGroup2:
return joint->limot2.get( parameter & 0xff );
break;
case dParamGroup3:
return joint->limotP.get( parameter & 0xff );
break;
}
return 0;
}
dJointType
dxJointPU::type() const
{
return dJointTypePU;
}
sizeint
dxJointPU::size() const
{
return sizeof( *this );
}
void
dxJointPU::setRelativeValues()
{
dVector3 anchor;
dJointGetPUAnchor(this, anchor);
setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
dVector3 ax1, ax2, ax3;
dJointGetPUAxis1(this, ax1);
dJointGetPUAxis2(this, ax2);
dJointGetPUAxis3(this, ax3);
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 );
}
setAxes( this, ax3[0], ax3[1], ax3[2], axisP1, NULL );
computeInitialRelativeRotations();
}