phyballandsocket.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 "phyballandsocket.h"
#include "private/phyjointprivate.h"
#include "private/phyobjectprivate.h"
#include "private/worldprivate.h"

namespace farsa {

PhyBallAndSocket::PhyBallAndSocket( const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
    : PhyJoint( parent, child ) {
    dofv = 0;
    forceOnJoint = wVector(0.0, 0.0, 0.0);

    //--- calculate the localMatrixParent
    localMatrixParent.x_ax = wVector::X();
    localMatrixParent.y_ax = wVector::Y();
    localMatrixParent.z_ax = wVector::Z();
    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 ( !localMatrixParent.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }
    if ( !localMatrixChild.sanityCheck() ) {
        qWarning( "Sanity Check Failed" );
    }
    
    if ( cp ) {
        createPrivateJoint();
    }
}

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

wVector PhyBallAndSocket::getForceOnJoint() const
{
    return forceOnJoint;
}

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

void PhyBallAndSocket::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" );
    }
}

void PhyBallAndSocket::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" );
    }

#ifdef WORLDSIM_USE_NEWTON
    UNUSED_PARAM( timestep );
    //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
    NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.x_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
    NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.y_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
    NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.z_ax[0] );
    NewtonUserJointSetRowStiffness( priv->joint, 1.0f );

    //--- Retrive forces applied to the joint by the constraints
    forceOnJoint.x = NewtonUserJointGetRowForce (priv->joint, 0);
    forceOnJoint.y = NewtonUserJointGetRowForce (priv->joint, 1);
    forceOnJoint.z = NewtonUserJointGetRowForce (priv->joint, 2);
#endif

}

} // end namespace farsa