blender/source/gameengine/Physics/BlOde/OdePhysicsEnvironment.cpp

234 lines
6.0 KiB
C++
Raw Normal View History

/**
* $Id$
*
* ***** 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>
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);
}
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 float* pos = dBodyGetPosition(bodyid1);
const float* R = dBodyGetRotation(bodyid1);
float offset[3] = {pivotX,pivotY,pivotZ};
float 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 float* pos = dBodyGetPosition(bodyid1);
const float* R = dBodyGetRotation(bodyid1);
float offset[3] = {pivotX,pivotY,pivotZ};
float axisset[3] = {axisX,axisY,axisZ};
float newoffset[3];
float 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;
}