Logo Search packages:      
Sourcecode: blender version File versions

OdePhysicsEnvironment.cpp

/**
 * $Id: OdePhysicsEnvironment.cpp,v 1.4 2004/03/22 22:02:07 jesterking Exp $
 *
 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
 *
 * The contents of this file may be used under the terms of either the GNU
 * General Public License Version 2 or later (the "GPL", see
 * http://www.gnu.org/licenses/gpl.html ), or the Blender License 1.0 or
 * later (the "BL", see http://www.blender.org/BL/ ) which has to be
 * bought from the Blender Foundation to become active, in which case the
 * above mentioned GPL option does not apply.
 *
 * The Original Code is Copyright (C) 2002 by NaN Holding BV.
 * All rights reserved.
 *
 * The Original Code is: all of this file.
 *
 * Contributor(s): none yet.
 *
 * ***** END GPL/BL DUAL LICENSE BLOCK *****
 */
#include "OdePhysicsEnvironment.h"
#include "PHY_IMotionState.h"
#include "OdePhysicsController.h"

// Ode
#include <ode/config.h>
#include <ode/ode.h>
#include <../ode/src/joint.h>
#include <ode/odemath.h>

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

00036 ODEPhysicsEnvironment::ODEPhysicsEnvironment()
{
      m_OdeWorld = dWorldCreate();
      m_OdeSpace = dHashSpaceCreate();
      m_OdeContactGroup = dJointGroupCreate (0);
      dWorldSetCFM (m_OdeWorld,1e-5f);

      m_JointGroup = dJointGroupCreate(0);
      
}



ODEPhysicsEnvironment::~ODEPhysicsEnvironment()
{
      dJointGroupDestroy (m_OdeContactGroup);
      dJointGroupDestroy (m_JointGroup);

      dSpaceDestroy (m_OdeSpace);
      dWorldDestroy (m_OdeWorld);
}

00058 void ODEPhysicsEnvironment::proceed(double timeStep)
{
      // ode collision update
      dSpaceCollide (m_OdeSpace,this,&ODEPhysicsEnvironment::OdeNearCallback);

      int m_odeContacts = GetNumOdeContacts();
      
      //physics integrator + resolver update
      dWorldStep (m_OdeWorld,timeStep);
      
      //clear collision points
      this->ClearOdeContactGroup();
}

void ODEPhysicsEnvironment::setGravity(float x,float y,float z)
{
      dWorldSetGravity (m_OdeWorld,x,y,z);
}



int               ODEPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
            float pivotX,float pivotY,float pivotZ,float axisX,float axisY,float axisZ)
{

      int constraintid = 0;
      ODEPhysicsController* dynactrl = (ODEPhysicsController*)ctrl;
      ODEPhysicsController* dynactrl2 = (ODEPhysicsController*)ctrl2;

      switch (type)
      {
      case PHY_POINT2POINT_CONSTRAINT:
            {
                  if (dynactrl)
                  {
                        dJointID jointid = dJointCreateBall (m_OdeWorld,m_JointGroup);
                        struct      dxBody*     bodyid1 = dynactrl->GetOdeBodyId();
                        struct      dxBody*     bodyid2=0;
                        const dReal* pos = dBodyGetPosition(bodyid1);
                        const dReal* R = dBodyGetRotation(bodyid1);
                        dReal offset[3] = {pivotX,pivotY,pivotZ};
                        dReal newoffset[3];
                        dMULTIPLY0_331 (newoffset,R,offset);
                        newoffset[0] += pos[0];
                        newoffset[1] += pos[1];
                        newoffset[2] += pos[2];
                        

                        if (dynactrl2)
                              bodyid2 = dynactrl2->GetOdeBodyId();
                        
                        dJointAttach (jointid, bodyid1, bodyid2);
                        
                        dJointSetBallAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
                        
                        constraintid = (int) jointid;
                  }
                  break;
            }
      case PHY_LINEHINGE_CONSTRAINT:
      {
                  if (dynactrl)
                  {
                        dJointID jointid = dJointCreateHinge (m_OdeWorld,m_JointGroup);
                        struct      dxBody*     bodyid1 = dynactrl->GetOdeBodyId();
                        struct      dxBody*     bodyid2=0;
                        const dReal* pos = dBodyGetPosition(bodyid1);
                        const dReal* R = dBodyGetRotation(bodyid1);
                        dReal offset[3] = {pivotX,pivotY,pivotZ};
                        dReal axisset[3] = {axisX,axisY,axisZ};
                        
                        dReal newoffset[3];
                        dReal newaxis[3];
                        dMULTIPLY0_331 (newaxis,R,axisset);
                        
                        dMULTIPLY0_331 (newoffset,R,offset);
                        newoffset[0] += pos[0];
                        newoffset[1] += pos[1];
                        newoffset[2] += pos[2];
                        

                        if (dynactrl2)
                              bodyid2 = dynactrl2->GetOdeBodyId();
                        
                        dJointAttach (jointid, bodyid1, bodyid2);
                        
                        dJointSetHingeAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
                        dJointSetHingeAxis(jointid,newaxis[0],newaxis[1],newaxis[2]);

                        constraintid = (int) jointid;
                  }
                  break;
            }
      default:
            {
                  //not yet
            }
      }
      
      return constraintid;

}

void        ODEPhysicsEnvironment::removeConstraint(int constraintid)
{
      if (constraintid)
      {
            dJointDestroy((dJointID) constraintid);
      }
}

PHY_IPhysicsController* ODEPhysicsEnvironment::rayTest(void* ignoreClient,float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
                                                      float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
{
      //collision detection / raytesting
      return NULL;
}


void ODEPhysicsEnvironment::OdeNearCallback (void *data, dGeomID o1, dGeomID o2)
{
      // \todo if this is a registered collision sensor
      // fire the callback

      int i;
      // if (o1->body && o2->body) return;
      ODEPhysicsEnvironment* env = (ODEPhysicsEnvironment*) data;
      dBodyID b1,b2;
      
      b1 = dGeomGetBody(o1);
      b2 = dGeomGetBody(o2);
      // exit without doing anything if the two bodies are connected by a joint
      if (b1 && b2 && dAreConnected (b1,b2)) return;

      ODEPhysicsController * ctrl1 =(ODEPhysicsController *)dGeomGetData(o1);
      ODEPhysicsController * ctrl2 =(ODEPhysicsController *)dGeomGetData(o2);
      float friction=ctrl1->getFriction();
      float restitution = ctrl1->getRestitution();
      //for friction, take minimum

      friction=(friction < ctrl2->getFriction() ?  
      friction :ctrl2->getFriction());

      //restitution:take minimum
      restitution = restitution < ctrl2->getRestitution()?
      restitution : ctrl2->getRestitution();

      dContact contact[3];                // up to 3 contacts per box
      for (i=0; i<3; i++) {
            contact[i].surface.mode = dContactBounce; //dContactMu2;
            contact[i].surface.mu = friction;//dInfinity;
            contact[i].surface.mu2 = 0;
            contact[i].surface.bounce = restitution;//0.5;
            contact[i].surface.bounce_vel = 0.1f;
            contact[i].surface.slip1=0.0;
      }
      
      if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
            // dMatrix3 RI;
            // dRSetIdentity (RI);
            // const dReal ss[3] = {0.02,0.02,0.02};
            for (i=0; i<numc; i++) {
                  dJointID c = dJointCreateContact (env->m_OdeWorld,env->m_OdeContactGroup,contact+i);
                  dJointAttach (c,b1,b2);
            }
      }
}


void  ODEPhysicsEnvironment::ClearOdeContactGroup()
{
      dJointGroupEmpty (m_OdeContactGroup);
}

int   ODEPhysicsEnvironment::GetNumOdeContacts()
{
      return m_OdeContactGroup->num;
}


Generated by  Doxygen 1.6.0   Back to index