path: root/libs/ode-0.16.1/ode/src/joints/hinge.cpp

/*************************************************************************
 *                                                                       *
 * 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 "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];
        }
    }
}