source/viewer/instructions/ListBody2BodyForces.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2007, 2008, 2009, University of Karlsruhe (for members see AUTHORS file)
 *
 * ISC License
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 * Created by Alexander Bierbaum
 */

#include "../include/IPSA/ipsaclasses.h"
#include "../include/IPSA/viewer/IpsaViewer.h"
#include "../include/IPSA/viewer/instructions/ListBody2BodyForces.h"

#include <ode/ode.h>

#include <sstream>

Instruction::tExecResultType ListBody2BodyForces::vExecute(std::vector<std::string>& Instr, std::ostringstream& answer, unsigned int& numberOfReturnValues)
{
    numberOfReturnValues = 0;
    SoNode* node = SoNode::getByName(Instr[1].c_str());
    SoNode* node2 = SoNode::getByName(Instr[2].c_str());

    if ((NULL == node) || (NULL == node2))
        return eBODY_NOT_FOUND;

    if ( !(node->isOfType(SoPhysics::getClassTypeId())) || !(node2->isOfType(SoPhysics::getClassTypeId())))
        return eNODE_IS_NOT_A_BODY;

    SoPhysics*  phy = static_cast<SoPhysics*>(node);
    dBodyID body1 = phy->getBody();
    SoPhysics*  phy2 = static_cast<SoPhysics*>(node2);
    dBodyID body2 = phy2->getBody();

    if ((NULL == body1) || (NULL == body2))
        return eNO_ODE_BODY_FOUND;

    int numberOfJoints = dBodyGetNumJoints(body1);
    int numberOfContacts = 0;
    tExecResultType result = eOK;

    // iterate over all joints connected to body1
    for (int i = 0; i < numberOfJoints; i++)
    {
        dJointID joint = dBodyGetJoint(body1, i);
        dBodyID contactBody1 = dJointGetBody(joint, 0);
        dBodyID contactBody2 = dJointGetBody(joint, 1);
        int jointType = dJointGetType(joint);
        if ((jointType == dJointTypeContact) && (contactBody2 == body2))
        {
            numberOfContacts++;
            dJointFeedback* jointFeedback = dJointGetFeedback(joint);
            if (NULL != jointFeedback)
            {
                dBodyID collidingBody;
                SoPhysics* ncb = NULL;
                if (contactBody1 == body1)      // find collision partner cb of body b
                    collidingBody = contactBody2;
                else
                    collidingBody = contactBody1;
                if (NULL != collidingBody)
                    ncb = static_cast<SoPhysics*>(dBodyGetData(collidingBody));
                answer << "contact force <";
                if (NULL != ncb)
                    answer << ncb->getName().getString();
                answer << "> "
                       << std::scientific << jointFeedback->f1[0]
                       << ' ' << jointFeedback->f1[1]
                       << ' ' << jointFeedback->f1[2]
                       << '\r' << std::endl;
                result = eOK;
                numberOfReturnValues += 3;
            }
            else
            {
                answer << "Body1 provides no force feedback! ";
                result = eUNKNOWN_ERROR;
                numberOfReturnValues = 0;
            }
        }
    }

    if (numberOfContacts == 0)
        answer << "no contacts found!\r" << std::endl;
    answer << "END\r" << std::endl; //End string, needed for Matlab

    return result;
}

---