first checkin of ode blender engine files

source/gameengine/Physics/BlOde/OdePhysicsEnvironment.cpp

@@ -0,0 +1,233 @@
+/**
+ * $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;
+}
+