← Back to branch summary

~diresu/blender/blender-command-port

« back to all changes in this revision

Viewing changes to source/gameengine/Physics/BlOde/OdePhysicsEnvironment.cpp

  • Committer: theeth
  • Date: 2008-10-14 16:52:04 UTC
  • Revision ID: vcs-imports@canonical.com-20081014165204-r32w2gm6s0osvdhn
copy back trunk

Show diffs side-by-side

added added

removed removed

Lines of Context:
source/gameengine/Physics/BlOde/OdePhysicsEnvironment.cpp
 
1
/**
 
2
 * $Id: OdePhysicsEnvironment.cpp 16596 2008-09-18 16:43:31Z ton $
 
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