worldsim/src/phyuniversal.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program 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               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#include "phyuniversal.h"
#include "private/phyjointprivate.h"
#include "private/phyobjectprivate.h"
#include "private/worldprivate.h"

namespace farsa {

PhyUniversal::PhyUniversal( const wVector& firstAxis, const wVector& secondAxis, const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
    : PhyJoint( parent, child ) {
    firstDof = new PhyDOF( this, firstAxis, centre, false );
    dofsv << firstDof;
    secondDof = new PhyDOF( this, secondAxis, centre, false );
    dofsv << secondDof;
    dofv = 2;
    //--- calculate the localMatrixParent
    //--- supposing that firstAxis and secondAxis are ortogonals
    localMatrixParent.x_ax = secondAxis;
    localMatrixParent.y_ax = firstAxis * secondAxis;
    localMatrixParent.z_ax = firstAxis;
    localMatrixParent.w_pos = centre;
    localMatrixParent.sanitifize();
    
    //--- calculate the local matrix respect to child object
    if ( parent ) {
        globalMatrixParent = localMatrixParent * parent->matrix();
    } else {
        globalMatrixParent = localMatrixParent;
    }
    localMatrixChild = globalMatrixParent * child->matrix().inverse();
    if ( parent ) {
        globalMatrixParent = localMatrixParent * parent->matrix();
    } else {
        globalMatrixParent = localMatrixParent;
    }

    firstDof->setAxis( globalMatrixParent.z_ax );
    firstDof->setXAxis( globalMatrixParent.x_ax );
    firstDof->setYAxis( globalMatrixParent.y_ax );
    firstDof->setCentre( globalMatrixParent.w_pos );

    globalMatrixChild = localMatrixChild * child->matrix();
    secondDof->setAxis( globalMatrixParent.x_ax );
    secondDof->setXAxis( globalMatrixParent.z_ax );
    secondDof->setYAxis( globalMatrixParent.y_ax );
    secondDof->setCentre( globalMatrixParent.w_pos );
    
    if ( !localMatrixParent.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }
    if ( !localMatrixChild.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }
    
    if ( cp ) {
        createPrivateJoint();
    }
}

wVector PhyUniversal::centre() {
    //--- calculate global matrix if necessary
    if ( parent() ) {
        globalMatrixParent = localMatrixParent * parent()->matrix();
    }
    return globalMatrixParent.w_pos;
};

void PhyUniversal::createPrivateJoint() {
#ifdef WORLDSIM_USE_NEWTON
    priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
    NewtonJointSetUserData( priv->joint, this );
#endif
}

void PhyUniversal::updateJointInfo() {
    //--- calculate the global matrices
    //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
    //--- there is no need to re-assign it because it was already done in constructor
    if ( parent() ) {
        globalMatrixParent = localMatrixParent * parent()->matrix();
    }
    globalMatrixChild = localMatrixChild * child()->matrix();

    if ( !globalMatrixParent.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }
    if ( !globalMatrixChild.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }

    firstDof->setAxis( globalMatrixParent.z_ax );
    firstDof->setXAxis( globalMatrixParent.x_ax );
    firstDof->setYAxis( globalMatrixParent.y_ax );
    firstDof->setCentre( globalMatrixParent.w_pos );

    //--- calculate the rotation assuming the local X axis of joint frame as 0
    //--- and following the right-hand convention
    real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
    real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
    real angle = atan2( sinAngle, cosAngle );
    firstDof->setPosition( angle );

    //--- the secondDOF results rotate of angle respect to its local X axis
    wMatrix tmp = wMatrix::identity();
    tmp.x_ax = globalMatrixParent.z_ax;
    tmp.y_ax = globalMatrixParent.y_ax;
    tmp.z_ax = globalMatrixParent.x_ax;
    tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
    tmp.sanitifize();
    secondDof->setAxis( tmp.z_ax );
    secondDof->setXAxis( tmp.x_ax );
    secondDof->setYAxis( tmp.y_ax );
    secondDof->setCentre( globalMatrixParent.w_pos );
}

void PhyUniversal::updateJoint( real timestep ) {
    //--- calculate the global matrices
    //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
    //--- there is no need to re-assign it because it was already done in constructor
    if ( parent() ) {
        globalMatrixParent = localMatrixParent * parent()->matrix();
    }
    globalMatrixChild = localMatrixChild * child()->matrix();

    if ( !globalMatrixParent.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }
    if ( !globalMatrixChild.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }

    firstDof->setAxis( globalMatrixParent.z_ax );
    firstDof->setXAxis( globalMatrixParent.x_ax );
    firstDof->setYAxis( globalMatrixParent.y_ax );
    firstDof->setCentre( globalMatrixParent.w_pos );

    //--- information about secondDOF will be calculated below
    //--- because it depends on rotation about firstDOF

#ifdef WORLDSIM_USE_NEWTON
    //--- get the pin fixed to the first body
    const wVector& dir0 = globalMatrixChild.z_ax;
    //--- get the pin fixed to the second body
    const wVector& dir1 = globalMatrixParent.x_ax;

    //--- construct an orthogonal coordinate system with these two vectors
    wVector dir2 = ( dir0 * dir1 );
    dir2.normalize();
    wVector dir3( dir2 * dir0 );
    dir3.normalize();

    const wVector& p0 = globalMatrixChild.w_pos;
    const wVector& p1 = globalMatrixParent.w_pos;

    real len = 100.0;
    wVector q0( p0 + dir3.scale(len) );
    wVector q1( p1 + dir1.scale(len) );

    //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
    NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir0[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
    NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir1[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
    NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir2[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
    //NewtonUserJointAddAngularRow( priv->joint, 0.0, &dir2[0] );
    //NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
    NewtonUserJointAddLinearRow( priv->joint, &q0[0], &q1[0], &dir0[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0 );

    //--- force will be used to set-up max and min torque newton will use to
    //--- resolve the constraint and accel to the double of appliedForce
    //--- this means that Newton will apply exactly forse amount of torque around
    //--- the axis
    real force = fabs( firstDof->appliedForce() );
    real accel = 2.0*firstDof->appliedForce();

    //--- calculate the rotation assuming the local X axis of joint frame as 0
    //--- and following the right-hand convention
    real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
    real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
    real angle = atan2( sinAngle, cosAngle );
    real vel = (angle - firstDof->position()) / timestep;
    firstDof->setPosition( angle );
    firstDof->setVelocity( vel );
    if ( firstDof->isLimited() ) {
        real lolimit;
        real hilimit;
        firstDof->limits( lolimit, hilimit );
        if ( angle < lolimit ) {
            real relAngle = lolimit - angle; // - lolimit;
            NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
            NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
            //--- this allow the joint to move back freely
            NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
        } else if ( angle > hilimit ) {
            real relAngle = hilimit - angle; // - hilimit;
            NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
            NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
            //--- this allow the joint to move forth freely
            NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
        } else {
            NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
            NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
            NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
            NewtonUserJointSetRowAcceleration( priv->joint, accel );
            NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
        }
    } else {
        NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
        NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
        NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
        NewtonUserJointSetRowAcceleration( priv->joint, accel );
        NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
    }

    //--- the secondDOF results rotate of angle respect to its local X axis
    wMatrix tmp = wMatrix::identity();
    tmp.x_ax = globalMatrixParent.z_ax;
    tmp.y_ax = globalMatrixParent.y_ax;
    tmp.z_ax = globalMatrixParent.x_ax;
    tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
    tmp.sanitifize();
    secondDof->setAxis( tmp.z_ax );
    secondDof->setXAxis( tmp.x_ax );
    secondDof->setYAxis( tmp.y_ax );
    secondDof->setCentre( globalMatrixParent.w_pos );
//  secondDof->setAxis( globalMatrixChild.x_ax );
//  secondDof->setXAxis( globalMatrixParent.z_ax );
//  secondDof->setYAxis( globalMatrixChild.y_ax );
//  secondDof->setCentre( globalMatrixParent.w_pos );   

    //--- force will be used to set-up max and min torque newton will use to
    //--- resolve the constraint and accel to the double of appliedForce
    //--- this means that Newton will apply exactly forse amount of torque around
    //--- the axis
    force = fabs( secondDof->appliedForce() );
    accel = 2.0*secondDof->appliedForce();

    //--- calculate the rotation assuming the local X axis of joint frame as 0
    //--- and following the right-hand convention
    //----------- Not sure check the second Axis rotation reference
    sinAngle = (globalMatrixParent.z_ax * globalMatrixChild.z_ax) % globalMatrixChild.x_ax;
    cosAngle = globalMatrixChild.z_ax % globalMatrixParent.z_ax;
    angle = atan2( sinAngle, cosAngle );
    vel = (angle - secondDof->position()) / timestep;
    secondDof->setPosition( angle );
    secondDof->setVelocity( vel );
    if ( secondDof->isLimited() ) {
        real lolimit;
        real hilimit;
        secondDof->limits( lolimit, hilimit );
        if ( angle < lolimit ) {
            real relAngle = lolimit - angle; // - lolimit;
            NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
            NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
            //--- this allow the joint to move back freely
            NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
        } else if ( angle > hilimit ) {
            real relAngle = hilimit - angle; // - hilimit;
            NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
            NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
            //--- this allow the joint to move forth freely
            NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
        } else {
            NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
            NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
            NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
            NewtonUserJointSetRowAcceleration( priv->joint, accel );
            NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
        }
    } else {
        NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
        NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
        NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
        NewtonUserJointSetRowAcceleration( priv->joint, accel );
        NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
    }
    
#endif

}

} // end namespace farsa