Fix compile errors for MSVC
[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 bool 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         return true;
72 }
73
74 void ODEPhysicsEnvironment::setGravity(float x,float y,float z)
75 {
76         dWorldSetGravity (m_OdeWorld,x,y,z);
77 }
78
79
80
81 int                     ODEPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
82                 float pivotX,float pivotY,float pivotZ,float axisX,float axisY,float axisZ)
83 {
84
85         int constraintid = 0;
86         ODEPhysicsController* dynactrl = (ODEPhysicsController*)ctrl;
87         ODEPhysicsController* dynactrl2 = (ODEPhysicsController*)ctrl2;
88
89         switch (type)
90         {
91         case PHY_POINT2POINT_CONSTRAINT:
92                 {
93                         if (dynactrl)
94                         {
95                                 dJointID jointid = dJointCreateBall (m_OdeWorld,m_JointGroup);
96                                 struct  dxBody* bodyid1 = dynactrl->GetOdeBodyId();
97                                 struct  dxBody* bodyid2=0;
98                                 const dReal* pos = dBodyGetPosition(bodyid1);
99                                 const dReal* R = dBodyGetRotation(bodyid1);
100                                 dReal offset[3] = {pivotX,pivotY,pivotZ};
101                                 dReal newoffset[3];
102                                 dMULTIPLY0_331 (newoffset,R,offset);
103                                 newoffset[0] += pos[0];
104                                 newoffset[1] += pos[1];
105                                 newoffset[2] += pos[2];
106                                 
107
108                                 if (dynactrl2)
109                                         bodyid2 = dynactrl2->GetOdeBodyId();
110                                 
111                                 dJointAttach (jointid, bodyid1, bodyid2);
112                                 
113                                 dJointSetBallAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
114                                 
115                                 constraintid = (int) jointid;
116                         }
117                         break;
118                 }
119         case PHY_LINEHINGE_CONSTRAINT:
120         {
121                         if (dynactrl)
122                         {
123                                 dJointID jointid = dJointCreateHinge (m_OdeWorld,m_JointGroup);
124                                 struct  dxBody* bodyid1 = dynactrl->GetOdeBodyId();
125                                 struct  dxBody* bodyid2=0;
126                                 const dReal* pos = dBodyGetPosition(bodyid1);
127                                 const dReal* R = dBodyGetRotation(bodyid1);
128                                 dReal offset[3] = {pivotX,pivotY,pivotZ};
129                                 dReal axisset[3] = {axisX,axisY,axisZ};
130                                 
131                                 dReal newoffset[3];
132                                 dReal newaxis[3];
133                                 dMULTIPLY0_331 (newaxis,R,axisset);
134                                 
135                                 dMULTIPLY0_331 (newoffset,R,offset);
136                                 newoffset[0] += pos[0];
137                                 newoffset[1] += pos[1];
138                                 newoffset[2] += pos[2];
139                                 
140
141                                 if (dynactrl2)
142                                         bodyid2 = dynactrl2->GetOdeBodyId();
143                                 
144                                 dJointAttach (jointid, bodyid1, bodyid2);
145                                 
146                                 dJointSetHingeAnchor (jointid, newoffset[0], newoffset[1], newoffset[2]);
147                                 dJointSetHingeAxis(jointid,newaxis[0],newaxis[1],newaxis[2]);
148
149                                 constraintid = (int) jointid;
150                         }
151                         break;
152                 }
153         default:
154                 {
155                         //not yet
156                 }
157         }
158         
159         return constraintid;
160
161 }
162
163 void            ODEPhysicsEnvironment::removeConstraint(int constraintid)
164 {
165         if (constraintid)
166         {
167                 dJointDestroy((dJointID) constraintid);
168         }
169 }
170
171 PHY_IPhysicsController* ODEPhysicsEnvironment::rayTest(void* ignoreClient,float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
172                                                                         float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
173 {
174         //collision detection / raytesting
175         return NULL;
176 }
177
178
179 void ODEPhysicsEnvironment::OdeNearCallback (void *data, dGeomID o1, dGeomID o2)
180 {
181         // \todo if this is a registered collision sensor
182         // fire the callback
183
184         int i;
185         // if (o1->body && o2->body) return;
186         ODEPhysicsEnvironment* env = (ODEPhysicsEnvironment*) data;
187         dBodyID b1,b2;
188         
189         b1 = dGeomGetBody(o1);
190         b2 = dGeomGetBody(o2);
191         // exit without doing anything if the two bodies are connected by a joint
192         if (b1 && b2 && dAreConnected (b1,b2)) return;
193
194         ODEPhysicsController * ctrl1 =(ODEPhysicsController *)dGeomGetData(o1);
195         ODEPhysicsController * ctrl2 =(ODEPhysicsController *)dGeomGetData(o2);
196         float friction=ctrl1->getFriction();
197         float restitution = ctrl1->getRestitution();
198         //for friction, take minimum
199
200         friction=(friction < ctrl2->getFriction() ?  
201         friction :ctrl2->getFriction());
202
203         //restitution:take minimum
204         restitution = restitution < ctrl2->getRestitution()?
205         restitution : ctrl2->getRestitution();
206
207         dContact contact[3];                    // up to 3 contacts per box
208         for (i=0; i<3; i++) {
209                 contact[i].surface.mode = dContactBounce; //dContactMu2;
210                 contact[i].surface.mu = friction;//dInfinity;
211                 contact[i].surface.mu2 = 0;
212                 contact[i].surface.bounce = restitution;//0.5;
213                 contact[i].surface.bounce_vel = 0.1f;
214                 contact[i].surface.slip1=0.0;
215         }
216         
217         if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
218                 // dMatrix3 RI;
219                 // dRSetIdentity (RI);
220                 // const dReal ss[3] = {0.02,0.02,0.02};
221                 for (i=0; i<numc; i++) {
222                         dJointID c = dJointCreateContact (env->m_OdeWorld,env->m_OdeContactGroup,contact+i);
223                         dJointAttach (c,b1,b2);
224                 }
225         }
226 }
227
228
229 void    ODEPhysicsEnvironment::ClearOdeContactGroup()
230 {
231         dJointGroupEmpty (m_OdeContactGroup);
232 }
233
234 int     ODEPhysicsEnvironment::GetNumOdeContacts()
235 {
236         return m_OdeContactGroup->num;
237 }
238