source/viewer/instructions/ListBody2BodyContacts.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2007, 2008, 2009, University of Karlsruhe (for members see AUTHORS file)
 * Copyright (c) 2012, Karlsruhe Institute of Technology (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/instructions/ListBody2BodyContacts.h"

#include <sstream>

#ifdef ODE_SOURCE
#include <ode/ode.h>
// include for defining dxJointContact:
#ifdef ODE_0_9
                #include <../ode/src/joint.h>                                    // ODE 0.9, versions below not supported
#else  // for ODE >= 0.10
                #include <../ode/src/joints/joints.h>    // for ODE >= 0.10
#endif 
#endif // ODE_SOURCE

Instruction::tExecResultType ListBody2BodyContacts::vExecute(std::vector<std::string>& Instr, std::ostringstream& answer, unsigned int& numberOfReturnValues)
{
    numberOfReturnValues = 0;
    tExecResultType result = eBODY_NOT_FOUND;
    /*
     * ListBody2BodyContacts requires the ODE 0.9 include file <joint.h>,
     * available only with the ODE source code.
     *
     */
#ifdef ODE_SOURCE
    SoNode * node = SoNode::getByName(Instr[1].c_str());
    SoNode * node2 = SoNode::getByName(Instr[2].c_str());
    int forceflag = atoi(Instr[3].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<SoPhyiscs*>(node);
    dBodyID body1 = phy->getBody();
    SoPhysics*  phy2 = static_cast<SoPhyiscs*>(node2);
    dBodyID body2 = phy2->getBody();
    if ((NULL == body1) || (NULL == body2))
        return eNO_ODE_BODY_FOUND;

    int numberOfJoints = dBodyGetNumJoints(body1);
    int numberOfContacts = 0;
    result = eOK;
    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++;
            dxJointContact* jc = static_cast<dxJointContact*>(joint);
            answer << "contact ";

            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 << "<";
            if (NULL != ncb)
                answer << ncb->getName().getString();
            answer << "> x= " << jc->contact.geom.pos[0]
                << " y= " << jc->contact.geom.pos[1]
                << " z= " << jc->contact.geom.pos[2]
                << " nx= " << jc->contact.geom.normal[0]
                << " ny= " << jc->contact.geom.normal[1]
                << " nz= " << jc->contact.geom.normal[2]
                << " d= " << jc->contact.geom.depth;
            numberOfReturnValues += 7;
            // get contact force data:
            if (forceflag)
            {
                dJointFeedback *jointFeedback = dJointGetFeedback(joint);
                if (NULL != jointFeedback)
                {
                    answer << " fx= " << jointFeedback->f1[0] << " fy= " << jointFeedback->f1[1] << " fz= " << jointFeedback->f1[2];
                    numberOfReturnValues += 3;
                }
                else
                {
                    answer << "Body1 provides no force feedback! ";
                    result = eUNKNOWN_ERROR;
                    numberOfReturnValues = 0;
                }
            }
            // end
            answer << "\r" << std::endl;
        }
    }

    if (numberOfContacts == 0)
        answer << "no contacts found\r"<< std::endl;
    answer << "END\r"<< std::endl; //End string, needed for Matlab
#else
    printf("Command not available, define ODE_SOURCE when building ipsaViewer-library !\n");
#endif // ODE_SOURCE
    return result;
}

---