Created a KX_SoftBodyDeformer for real-time soft bodies.
[blender.git] / source / gameengine / Physics / BlOde / OdePhysicsEnvironment.cpp
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL 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 LICENSE BLOCK *****
21  */
22 #include "OdePhysicsEnvironment.h"
23 #include "PHY_IMotionState.h"
24 #include "OdePhysicsController.h"
25
26 #include <ode/ode.h>
27 #include <../ode/src/joint.h>
28 #include <ode/odemath.h>
29
30 ODEPhysicsEnvironment::ODEPhysicsEnvironment()
31 {
32         m_OdeWorld = dWorldCreate();
33         m_OdeSpace = dHashSpaceCreate();
34         m_OdeContactGroup = dJointGroupCreate (0);
35         dWorldSetCFM (m_OdeWorld,1e-5f);
36
37         m_JointGroup = dJointGroupCreate(0);
38
39         setFixedTimeStep(true,1.f/60.f);
40 }
41
42
43
44 ODEPhysicsEnvironment::~ODEPhysicsEnvironment()
45 {
46         dJointGroupDestroy (m_OdeContactGroup);
47         dJointGroupDestroy (m_JointGroup);
48
49         dSpaceDestroy (m_OdeSpace);
50         dWorldDestroy (m_OdeWorld);
51 }
52
53
54
55 void            ODEPhysicsEnvironment::setFixedTimeStep(bool useFixedTimeStep,float fixedTimeStep)
56 {
57         m_useFixedTimeStep = useFixedTimeStep;
58
59         if (useFixedTimeStep)
60         {
61                 m_fixedTimeStep = fixedTimeStep;
62         } else
63         {
64                 m_fixedTimeStep = 0.f;
65         }
66         m_currentTime = 0.f;
67
68         //todo:implement fixed timestepping
69
70 }
71 float           ODEPhysicsEnvironment::getFixedTimeStep()
72 {
73         return m_fixedTimeStep;
74 }
75
76
77
78 bool            ODEPhysicsEnvironment::proceedDeltaTime(double  curTime,float timeStep1)
79 {
80
81         float deltaTime = timeStep1;
82         int     numSteps = 1;
83
84         if (m_useFixedTimeStep)
85         {
86                 m_currentTime += timeStep1;             
87                 // equal to subSampling (might be a little smaller).
88                 numSteps = (int)(m_currentTime / m_fixedTimeStep);
89                 m_currentTime -= m_fixedTimeStep * (float)numSteps;
90                 deltaTime = m_fixedTimeStep;
91                 //todo: experiment by smoothing the remaining time over the substeps
92         }
93
94         for (int i=0;i<numSteps;i++)
95         {
96                 // ode collision update
97                 dSpaceCollide (m_OdeSpace,this,&ODEPhysicsEnvironment::OdeNearCallback);
98
99                 int m_odeContacts = GetNumOdeContacts();
100                 
101                 //physics integrator + resolver update
102                 //dWorldStep (m_OdeWorld,deltaTime);
103                 //dWorldQuickStep (m_OdeWorld,deltaTime);
104                 //dWorldID w, dReal stepsize)
105
106                 //clear collision points
107                 this->ClearOdeContactGroup();
108         }
109         return true;
110 }
111
112 void ODEPhysicsEnvironment::setGravity(float x,float y,float z)
113 {
114         dWorldSetGravity (m_OdeWorld,x,y,z);
115 }
116
117
118
119 int                     ODEPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
120                 float pivotX,float pivotY,float pivotZ,float axisX,float axisY,float axisZ)
121 {
122
123         int constraintid = 0;
124         ODEPhysicsController* dynactrl = (ODEPhysicsController*)ctrl;
125         ODEPhysicsController* dynactrl2 = (ODEPhysicsController*)ctrl2;
126
127         switch (type)
128         {
129         case PHY_POINT2POINT_CONSTRAINT:
130                 {
131                         if (dynactrl)
132                         {
133                                 dJointID jointid = dJointCreateBall (m_OdeWorld,m_JointGroup);
134                                 struct  dxBody* bodyid1 = dynactrl->GetOdeBodyId();
135                                 struct  dxBody* bodyid2=0;
136                                 const dReal* pos = dBodyGetPosition(bodyid1);
137                                 const dReal* R = dBodyGetRotation(bodyid1);
138                                 dReal offset[3] = {pivotX,pivotY,pivotZ};
139                                 dReal newoffset[3];
140                                 dMULTIPLY0_331 (newoffset,R,offset);
141                                 newoffset[0] += pos[0];
142                                 newoffset[1] += pos[1];
143                                 newoffset[2] += pos[2];
144                                 
145
146                                 if (dynactrl2)
147                                         bodyid2 = dynactrl2->GetOdeBodyId();
148                                 
149                                 dJointAttach (jointid, bodyid1, bodyid2);
150                                 
151                                 dJointSetBallAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
152                                 
153                                 constraintid = (int) jointid;
154                         }
155                         break;
156                 }
157         case PHY_LINEHINGE_CONSTRAINT:
158         {
159                         if (dynactrl)
160                         {
161                                 dJointID jointid = dJointCreateHinge (m_OdeWorld,m_JointGroup);
162                                 struct  dxBody* bodyid1 = dynactrl->GetOdeBodyId();
163                                 struct  dxBody* bodyid2=0;
164                                 const dReal* pos = dBodyGetPosition(bodyid1);
165                                 const dReal* R = dBodyGetRotation(bodyid1);
166                                 dReal offset[3] = {pivotX,pivotY,pivotZ};
167                                 dReal axisset[3] = {axisX,axisY,axisZ};
168                                 
169                                 dReal newoffset[3];
170                                 dReal newaxis[3];
171                                 dMULTIPLY0_331 (newaxis,R,axisset);
172                                 
173                                 dMULTIPLY0_331 (newoffset,R,offset);
174                                 newoffset[0] += pos[0];
175                                 newoffset[1] += pos[1];
176                                 newoffset[2] += pos[2];
177                                 
178
179                                 if (dynactrl2)
180                                         bodyid2 = dynactrl2->GetOdeBodyId();
181                                 
182                                 dJointAttach (jointid, bodyid1, bodyid2);
183                                 
184                                 dJointSetHingeAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
185                                 dJointSetHingeAxis(jointid,newaxis[0],newaxis[1],newaxis[2]);
186
187                                 constraintid = (int) jointid;
188                         }
189                         break;
190                 }
191         default:
192                 {
193                         //not yet
194                 }
195         }
196         
197         return constraintid;
198
199 }
200
201 void            ODEPhysicsEnvironment::removeConstraint(void *constraintid)
202 {
203         if (constraintid)
204         {
205                 dJointDestroy((dJointID) constraintid);
206         }
207 }
208
209 PHY_IPhysicsController* ODEPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback,float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
210 {
211
212         //m_OdeWorld
213         //collision detection / raytesting
214         return NULL;
215 }
216
217
218 void ODEPhysicsEnvironment::OdeNearCallback (void *data, dGeomID o1, dGeomID o2)
219 {
220         // \todo if this is a registered collision sensor
221         // fire the callback
222
223         int i;
224         // if (o1->body && o2->body) return;
225         ODEPhysicsEnvironment* env = (ODEPhysicsEnvironment*) data;
226         dBodyID b1,b2;
227         
228         b1 = dGeomGetBody(o1);
229         b2 = dGeomGetBody(o2);
230         // exit without doing anything if the two bodies are connected by a joint
231         if (b1 && b2 && dAreConnected (b1,b2)) return;
232
233         ODEPhysicsController * ctrl1 =(ODEPhysicsController *)dGeomGetData(o1);
234         ODEPhysicsController * ctrl2 =(ODEPhysicsController *)dGeomGetData(o2);
235         float friction=ctrl1->getFriction();
236         float restitution = ctrl1->getRestitution();
237         //for friction, take minimum
238
239         friction=(friction < ctrl2->getFriction() ?  
240         friction :ctrl2->getFriction());
241
242         //restitution:take minimum
243         restitution = restitution < ctrl2->getRestitution()?
244         restitution : ctrl2->getRestitution();
245
246         dContact contact[3];                    // up to 3 contacts per box
247         for (i=0; i<3; i++) {
248                 contact[i].surface.mode = dContactBounce; //dContactMu2;
249                 contact[i].surface.mu = friction;//dInfinity;
250                 contact[i].surface.mu2 = 0;
251                 contact[i].surface.bounce = restitution;//0.5;
252                 contact[i].surface.bounce_vel = 0.1f;
253                 contact[i].surface.slip1=0.0;
254         }
255         
256         if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
257                 // dMatrix3 RI;
258                 // dRSetIdentity (RI);
259                 // const dReal ss[3] = {0.02,0.02,0.02};
260                 for (i=0; i<numc; i++) {
261                         dJointID c = dJointCreateContact (env->m_OdeWorld,env->m_OdeContactGroup,contact+i);
262                         dJointAttach (c,b1,b2);
263                 }
264         }
265 }
266
267
268 void    ODEPhysicsEnvironment::ClearOdeContactGroup()
269 {
270         dJointGroupEmpty (m_OdeContactGroup);
271 }
272
273 int     ODEPhysicsEnvironment::GetNumOdeContacts()
274 {
275         return m_OdeContactGroup->num;
276 }
277