7851a9f34f7985db006b668e02b8201704cab4c2
[blender-staging.git] / source / gameengine / Physics / BlOde / OdePhysicsEnvironment.cpp
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5  *
6  * The contents of this file may be used under the terms of either the GNU
7  * General Public License Version 2 or later (the "GPL", see
8  * http://www.gnu.org/licenses/gpl.html ), or the Blender License 1.0 or
9  * later (the "BL", see http://www.blender.org/BL/ ) which has to be
10  * bought from the Blender Foundation to become active, in which case the
11  * above mentioned GPL option does not apply.
12  *
13  * The Original Code is Copyright (C) 2002 by NaN Holding BV.
14  * All rights reserved.
15  *
16  * The Original Code is: all of this file.
17  *
18  * Contributor(s): none yet.
19  *
20  * ***** END GPL/BL DUAL LICENSE BLOCK *****
21  */
22 #include "OdePhysicsEnvironment.h"
23 #include "PHY_IMotionState.h"
24 #include "OdePhysicsController.h"
25
26 // Ode
27 #include <ode/config.h>
28 #include <ode/ode.h>
29 #include <../ode/src/joint.h>
30 #include <ode/odemath.h>
31
32 #ifdef HAVE_CONFIG_H
33 #include <config.h>
34 #endif
35
36 ODEPhysicsEnvironment::ODEPhysicsEnvironment()
37 {
38         m_OdeWorld = dWorldCreate();
39         m_OdeSpace = dHashSpaceCreate();
40         m_OdeContactGroup = dJointGroupCreate (0);
41         dWorldSetCFM (m_OdeWorld,1e-5f);
42
43         m_JointGroup = dJointGroupCreate(0);
44         
45 }
46
47
48
49 ODEPhysicsEnvironment::~ODEPhysicsEnvironment()
50 {
51         dJointGroupDestroy (m_OdeContactGroup);
52         dJointGroupDestroy (m_JointGroup);
53
54         dSpaceDestroy (m_OdeSpace);
55         dWorldDestroy (m_OdeWorld);
56 }
57
58 void ODEPhysicsEnvironment::proceed(double timeStep)
59 {
60         // ode collision update
61         dSpaceCollide (m_OdeSpace,this,&ODEPhysicsEnvironment::OdeNearCallback);
62
63         int m_odeContacts = GetNumOdeContacts();
64         
65         //physics integrator + resolver update
66         dWorldStep (m_OdeWorld,timeStep);
67         
68         //clear collision points
69         this->ClearOdeContactGroup();
70 }
71
72 void ODEPhysicsEnvironment::setGravity(float x,float y,float z)
73 {
74         dWorldSetGravity (m_OdeWorld,x,y,z);
75 }
76
77
78
79 int                     ODEPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
80                 float pivotX,float pivotY,float pivotZ,float axisX,float axisY,float axisZ)
81 {
82
83         int constraintid = 0;
84         ODEPhysicsController* dynactrl = (ODEPhysicsController*)ctrl;
85         ODEPhysicsController* dynactrl2 = (ODEPhysicsController*)ctrl2;
86
87         switch (type)
88         {
89         case PHY_POINT2POINT_CONSTRAINT:
90                 {
91                         if (dynactrl)
92                         {
93                                 dJointID jointid = dJointCreateBall (m_OdeWorld,m_JointGroup);
94                                 struct  dxBody* bodyid1 = dynactrl->GetOdeBodyId();
95                                 struct  dxBody* bodyid2=0;
96                                 const dReal* pos = dBodyGetPosition(bodyid1);
97                                 const dReal* R = dBodyGetRotation(bodyid1);
98                                 dReal offset[3] = {pivotX,pivotY,pivotZ};
99                                 dReal newoffset[3];
100                                 dMULTIPLY0_331 (newoffset,R,offset);
101                                 newoffset[0] += pos[0];
102                                 newoffset[1] += pos[1];
103                                 newoffset[2] += pos[2];
104                                 
105
106                                 if (dynactrl2)
107                                         bodyid2 = dynactrl2->GetOdeBodyId();
108                                 
109                                 dJointAttach (jointid, bodyid1, bodyid2);
110                                 
111                                 dJointSetBallAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
112                                 
113                                 constraintid = (int) jointid;
114                         }
115                         break;
116                 }
117         case PHY_LINEHINGE_CONSTRAINT:
118         {
119                         if (dynactrl)
120                         {
121                                 dJointID jointid = dJointCreateHinge (m_OdeWorld,m_JointGroup);
122                                 struct  dxBody* bodyid1 = dynactrl->GetOdeBodyId();
123                                 struct  dxBody* bodyid2=0;
124                                 const dReal* pos = dBodyGetPosition(bodyid1);
125                                 const dReal* R = dBodyGetRotation(bodyid1);
126                                 dReal offset[3] = {pivotX,pivotY,pivotZ};
127                                 dReal axisset[3] = {axisX,axisY,axisZ};
128                                 
129                                 dReal newoffset[3];
130                                 dReal newaxis[3];
131                                 dMULTIPLY0_331 (newaxis,R,axisset);
132                                 
133                                 dMULTIPLY0_331 (newoffset,R,offset);
134                                 newoffset[0] += pos[0];
135                                 newoffset[1] += pos[1];
136                                 newoffset[2] += pos[2];
137                                 
138
139                                 if (dynactrl2)
140                                         bodyid2 = dynactrl2->GetOdeBodyId();
141                                 
142                                 dJointAttach (jointid, bodyid1, bodyid2);
143                                 
144                                 dJointSetHingeAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
145                                 dJointSetHingeAxis(jointid,newaxis[0],newaxis[1],newaxis[2]);
146
147                                 constraintid = (int) jointid;
148                         }
149                         break;
150                 }
151         default:
152                 {
153                         //not yet
154                 }
155         }
156         
157         return constraintid;
158
159 }
160
161 void            ODEPhysicsEnvironment::removeConstraint(int constraintid)
162 {
163         if (constraintid)
164         {
165                 dJointDestroy((dJointID) constraintid);
166         }
167 }
168
169 PHY_IPhysicsController* ODEPhysicsEnvironment::rayTest(void* ignoreClient,float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
170                                                                         float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
171 {
172         //collision detection / raytesting
173         return NULL;
174 }
175
176
177 void ODEPhysicsEnvironment::OdeNearCallback (void *data, dGeomID o1, dGeomID o2)
178 {
179         // \todo if this is a registered collision sensor
180         // fire the callback
181
182         int i;
183         // if (o1->body && o2->body) return;
184         ODEPhysicsEnvironment* env = (ODEPhysicsEnvironment*) data;
185         dBodyID b1,b2;
186         
187         b1 = dGeomGetBody(o1);
188         b2 = dGeomGetBody(o2);
189         // exit without doing anything if the two bodies are connected by a joint
190         if (b1 && b2 && dAreConnected (b1,b2)) return;
191
192         ODEPhysicsController * ctrl1 =(ODEPhysicsController *)dGeomGetData(o1);
193         ODEPhysicsController * ctrl2 =(ODEPhysicsController *)dGeomGetData(o2);
194         float friction=ctrl1->getFriction();
195         float restitution = ctrl1->getRestitution();
196         //for friction, take minimum
197
198         friction=(friction < ctrl2->getFriction() ?  
199         friction :ctrl2->getFriction());
200
201         //restitution:take minimum
202         restitution = restitution < ctrl2->getRestitution()?
203         restitution : ctrl2->getRestitution();
204
205         dContact contact[3];                    // up to 3 contacts per box
206         for (i=0; i<3; i++) {
207                 contact[i].surface.mode = dContactBounce; //dContactMu2;
208                 contact[i].surface.mu = friction;//dInfinity;
209                 contact[i].surface.mu2 = 0;
210                 contact[i].surface.bounce = restitution;//0.5;
211                 contact[i].surface.bounce_vel = 0.1f;
212                 contact[i].surface.slip1=0.0;
213         }
214         
215         if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
216                 // dMatrix3 RI;
217                 // dRSetIdentity (RI);
218                 // const dReal ss[3] = {0.02,0.02,0.02};
219                 for (i=0; i<numc; i++) {
220                         dJointID c = dJointCreateContact (env->m_OdeWorld,env->m_OdeContactGroup,contact+i);
221                         dJointAttach (c,b1,b2);
222                 }
223         }
224 }
225
226
227 void    ODEPhysicsEnvironment::ClearOdeContactGroup()
228 {
229         dJointGroupEmpty (m_OdeContactGroup);
230 }
231
232 int     ODEPhysicsEnvironment::GetNumOdeContacts()
233 {
234         return m_OdeContactGroup->num;
235 }
236