source/viewer/instructions/ListBodyContacts.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/ListBodyContacts.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 ListBodyContacts::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());
    if (NULL == node)
        return eBODY_NOT_FOUND;

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

    SoPhysics*  phy = (SoPhysics *)node;
    dBodyID body = phy->getBody();
    if (NULL == body)
        return eNO_ODE_BODY_FOUND;

    int numberOfJoints = dBodyGetNumJoints(body);
    int numberOfContacts = 0;
    result = eOK;
    for (int i = 0; i < numberOfJoints; i++)
    {
        dJointID joint = dBodyGetJoint(body, i);
        int jointType = dJointGetType(joint);
        if (jointType == dJointTypeContact)
        {
            numberOfContacts++;
            dxJointContact* jointContact = static_cast<dxJointContact*>(joint);
            answer << "contact ";
            dBodyID contactBody1 = dJointGetBody(joint,0);
            dBodyID contactBody2 = dJointGetBody(joint,1);
            dBodyID collisionBody;
            SoPhysics* ncb = NULL;
            if (contactBody1 == body)      // find collision partner cb of body b
                collisionBody = contactBody2;
            else
                collisionBody = contactBody1;
            if (NULL != collisionBody)
                ncb = static_cast<SoPhysics*>(dBodyGetData(collisionBody));
            answer << "<";
            if (NULL != ncb)
                answer << ncb->getName().getString();
            answer << "> x= " << jointContact->contact.geom.pos[0]
                << " y= " << jointContact->contact.geom.pos[1]
                << " z= " << jointContact->contact.geom.pos[2]
                << " nx= " << jointContact->contact.geom.normal[0]
                << " ny= " << jointContact->contact.geom.normal[1]
                << " nz= " << jointContact->contact.geom.normal[2]
                << " d= " << jointContact->contact.geom.depth;
            numberOfReturnValues += 7;
        }
    }

    if (numberOfContacts == 0)
        answer << "no contacts found!";
    else 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;
}

---