Added resolveCombinedVelocities()
[blender.git] / source / gameengine / Physics / Sumo / Fuzzics / src / SM_Scene.cpp
1 /**
2  * $Id$
3  * Copyright (C) 2001 NaN Technologies B.V.
4  * The physics scene.
5  *
6  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
7  *
8  * This program is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * as published by the Free Software Foundation; either version 2
11  * of the License, or (at your option) any later version. The Blender
12  * Foundation also sells licenses for use in proprietary software under
13  * the Blender License.  See http://www.blender.org/BL/ for information
14  * about this.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software Foundation,
23  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
24  *
25  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
26  * All rights reserved.
27  *
28  * The Original Code is: all of this file.
29  *
30  * Contributor(s): none yet.
31  *
32  * ***** END GPL/BL DUAL LICENSE BLOCK *****
33  */
34
35 #ifdef HAVE_CONFIG_H
36 #include <config.h>
37 #endif
38
39 #ifdef WIN32
40 #pragma warning(disable : 4786)  // shut off 255 char limit debug template warning
41 #endif
42
43 #include "SM_Scene.h"
44 #include "SM_Object.h"
45 #include "SM_FhObject.h"
46
47 #include "SM_Debug.h"
48
49 #include <algorithm>
50
51 SM_Scene::SM_Scene() : 
52         m_scene(DT_CreateScene()),
53         m_respTable(DT_CreateRespTable()),
54         m_secondaryRespTable(DT_CreateRespTable()),
55         m_fixRespTable(DT_CreateRespTable()),
56         m_forceField(0.0, 0.0, 0.0)
57 {
58         for (int i = 0 ; i < NUM_RESPONSE; i++)
59         {
60                 m_ResponseClass[i] = DT_GenResponseClass(m_respTable);
61                 m_secondaryResponseClass[i] = DT_GenResponseClass(m_secondaryRespTable);
62                 m_fixResponseClass[i] = DT_GenResponseClass(m_fixRespTable);
63         }
64         
65         /* Sensor */
66         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
67         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
68         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
69         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
70         
71         /* Static */
72         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
73         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[STATIC_RESPONSE], 0, DT_NO_RESPONSE, this);
74         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this);
75         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
76         
77         /* Object */
78         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
79         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this);
80         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this);
81         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
82         
83         /* Fh Object */
84         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
85         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
86         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
87         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
88         
89         /* Object (Fix Pass) */
90         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
91         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::fix, DT_SIMPLE_RESPONSE, this);
92         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::fix, DT_SIMPLE_RESPONSE, this);
93         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
94 }
95
96 void SM_Scene::addTouchCallback(int response_class, DT_ResponseCallback callback, void *user)
97 {
98         DT_AddClassResponse(m_secondaryRespTable, m_secondaryResponseClass[response_class], callback, DT_SIMPLE_RESPONSE, user);
99 }
100
101 void SM_Scene::addSensor(SM_Object& object) 
102 {
103         object.calcXform();
104         m_objectList.push_back(&object);
105         DT_AddObject(m_scene, object.getObjectHandle());
106         DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[SENSOR_RESPONSE]);
107         DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[SENSOR_RESPONSE]);
108         DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[SENSOR_RESPONSE]);
109 }
110
111 void SM_Scene::add(SM_Object& object) {
112         object.calcXform();
113         m_objectList.push_back(&object);
114         DT_AddObject(m_scene, object.getObjectHandle());
115         if (object.isDynamic()) {
116                 DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]);
117                 DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]);
118                 DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]);
119         } else {
120                 DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[STATIC_RESPONSE]);
121                 DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[STATIC_RESPONSE]);
122                 DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[STATIC_RESPONSE]);
123         }       
124
125         SM_FhObject *fh_object = object.getFhObject();
126
127         if (fh_object) {
128                 DT_AddObject(m_scene, fh_object->getObjectHandle());
129                 DT_SetResponseClass(m_respTable, fh_object->getObjectHandle(), m_ResponseClass[FH_RESPONSE]);
130                 DT_SetResponseClass(m_secondaryRespTable, fh_object->getObjectHandle(), m_secondaryResponseClass[FH_RESPONSE]);
131                 DT_SetResponseClass(m_fixRespTable, fh_object->getObjectHandle(), m_fixResponseClass[FH_RESPONSE]);
132         }
133 }       
134
135 void SM_Scene::requestCollisionCallback(SM_Object &object)
136 {
137         DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]);
138         DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]);
139 //      DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]);
140 }
141
142 void SM_Scene::remove(SM_Object& object) {      
143         T_ObjectList::iterator i =
144                 std::find(m_objectList.begin(), m_objectList.end(), &object);
145         if (!(i == m_objectList.end()))
146         {
147                 std::swap(*i, m_objectList.back());
148                 m_objectList.pop_back();
149                 DT_RemoveObject(m_scene, object.getObjectHandle());
150
151                 SM_FhObject *fh_object = object.getFhObject();
152                 
153                 if (fh_object) {
154                         DT_RemoveObject(m_scene, fh_object->getObjectHandle());
155                 }
156         } 
157         else {
158                 // tried to remove an object that is not in the scene
159                 //assert(false);
160         }
161 }       
162
163 void SM_Scene::proceed(MT_Scalar timeStep, MT_Scalar subSampling) {
164         // Don't waste time...but it's OK to spill a little.
165         if (timeStep < 0.001)
166                 return;
167
168         // Divide the timeStep into a number of subsamples of size roughly 
169         // equal to subSampling (might be a little smaller).
170         int num_samples = (int)ceil(timeStep / subSampling);
171
172         MT_Scalar subStep = timeStep / num_samples;
173         T_ObjectList::iterator i;
174
175         // Apply a forcefield (such as gravity)
176         for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
177                 (*i)->applyForceField(m_forceField);
178                 //(*i)->integrateForces(subStep);
179                 //(*i)->integrateMomentum(subStep);
180         }
181         
182         // Do the integration steps per object.
183         int step;
184         for (step = 0; step != num_samples; ++step) {
185
186
187                 for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
188                         (*i)->integrateForces(subStep);
189                         //(*i)->backup();
190                 // And second we update the object positions by performing
191                 // an integration step for each object
192                         (*i)->integrateMomentum(subStep);
193                 }
194                 // I changed the order of the next 2 statements.
195                 // Originally objects were first integrated with a call
196                 // to proceed(). However if external objects were 
197                 // directly manipulating the velocities etc of physics 
198                 // objects then the physics environment would not be able 
199                 // to react before object positions were updated. --- Laurence.
200
201                 // So now first we let the physics scene respond to 
202                 // new forces, velocities set externally. 
203                 // The collsion and friction impulses are computed here. 
204                 DT_Test(m_scene, m_respTable);
205         }
206
207         // clear the user set velocities.
208 #if 0
209         clearObjectCombinedVelocities();
210 #endif
211         if (DT_Test(m_scene, m_fixRespTable))
212                 for (i = m_objectList.begin(); i != m_objectList.end(); ++i)
213                         (*i)->relax(); 
214         
215         // Finish this timestep by saving al state information for the next
216         // timestep and clearing the accumulated forces. 
217
218         for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
219                 (*i)->proceedKinematic(timeStep);
220                 (*i)->saveReactionForce(timeStep);
221                 (*i)->clearForce();
222         }
223
224         // For each pair of object that collided, call the corresponding callback.
225         // Additional collisions of a pair within the same time step are ignored.
226
227         if (m_secondaryRespTable) {
228                 T_PairList::iterator p;
229                 for (p = m_pairList.begin(); p != m_pairList.end(); ++p) {
230                         DT_CallResponse(m_secondaryRespTable, 
231                                                         (*p).first->getObjectHandle(), 
232                                                         (*p).second->getObjectHandle(), 
233                                                         0);
234                 }
235         }
236         
237         clearPairs();
238 }
239
240 SM_Object *SM_Scene::rayTest(void *ignore_client, 
241                                                          const MT_Point3& from, const MT_Point3& to, 
242                                                          MT_Point3& result, MT_Vector3& normal) const {
243 #ifdef SM_DEBUG_RAYCAST
244         std::cout << "ray: { " << from << " } - { " << to << " }" << std::endl; 
245 #endif
246  
247         DT_Vector3 n, dfrom, dto;
248         DT_Scalar param;
249         from.getValue(dfrom);
250         to.getValue(dto);
251         SM_Object *hit_object = (SM_Object *) 
252                 DT_RayCast(m_scene, ignore_client, dfrom, dto, 1., &param, n);
253
254         if (hit_object) {
255                 //result = hit_object->getWorldCoord(from + (to - from)*param);
256                 result = from + (to - from) * param;
257                 normal.setValue(n);
258 #ifdef SM_DEBUG_RAYCAST
259                 std::cout << "ray: { " << from << " } -> { " << to << " }: { " << result 
260                   << " } (" << param << "), normal = { " << normal << " }" << std::endl;
261 #endif
262         }
263
264         return hit_object;
265 }
266
267 void SM_Scene::clearObjectCombinedVelocities() {
268
269         T_ObjectList::iterator i;
270
271         for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
272
273                 (*i)->clearCombinedVelocities();
274
275         }
276
277 }       
278
279
280 void SM_Scene::setSecondaryRespTable(DT_RespTableHandle secondaryRespTable) {
281         m_secondaryRespTable = secondaryRespTable;
282 }
283
284
285 DT_Bool SM_Scene::boing(
286         void *client_data,  
287         void *object1,
288         void *object2,
289         const DT_CollData *coll_data
290 ){
291         SM_Scene  *scene = (SM_Scene *)client_data; 
292         SM_Object *obj1  = (SM_Object *)object1;  
293         SM_Object *obj2  = (SM_Object *)object2;  
294         
295         scene->addPair(obj1, obj2); // Record this collision for client callbacks
296
297 #ifdef SM_DEBUG_BOING   
298         printf("SM_Scene::boing\n");
299 #endif
300         
301         return DT_CONTINUE;
302 }
303
304 SM_Scene::~SM_Scene()
305
306 //      if (m_objectList.begin() != m_objectList.end()) 
307 //              std::cout << "SM_Scene::~SM_Scene: There are still objects in the Sumo scene!" << std::endl;
308         for (T_ObjectList::iterator it = m_objectList.begin() ; it != m_objectList.end() ; it++)
309                 delete *it;
310         
311         DT_DestroyRespTable(m_respTable);
312         DT_DestroyRespTable(m_secondaryRespTable);
313         DT_DestroyRespTable(m_fixRespTable);
314         DT_DestroyScene(m_scene);
315 }