Reverted incorrect merge (missing files)
[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 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.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software Foundation,
20  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21  *
22  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
23  * All rights reserved.
24  *
25  * The Original Code is: all of this file.
26  *
27  * Contributor(s): none yet.
28  *
29  * ***** END GPL LICENSE BLOCK *****
30  */
31
32 #ifdef HAVE_CONFIG_H
33 #include <config.h>
34 #endif
35
36 #ifdef WIN32
37 #pragma warning(disable : 4786)  // shut off 255 char limit debug template warning
38 #endif
39
40 #include "SM_Scene.h"
41 #include "SM_Object.h"
42 #include "SM_FhObject.h"
43
44 #include "SM_Debug.h"
45
46 #include <algorithm>
47
48 SM_Scene::SM_Scene() : 
49         m_scene(DT_CreateScene()),
50         m_respTable(DT_CreateRespTable()),
51         m_secondaryRespTable(DT_CreateRespTable()),
52         m_fixRespTable(DT_CreateRespTable()),
53         m_forceField(0.0, 0.0, 0.0),
54         m_frames(0)
55 {
56         for (int i = 0 ; i < NUM_RESPONSE; i++)
57         {
58                 m_ResponseClass[i] = DT_GenResponseClass(m_respTable);
59                 m_secondaryResponseClass[i] = DT_GenResponseClass(m_secondaryRespTable);
60                 m_fixResponseClass[i] = DT_GenResponseClass(m_fixRespTable);
61         }
62         
63         /* Sensor */
64         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
65         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
66         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
67         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
68         
69         /* Static */
70         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
71         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[STATIC_RESPONSE], 0, DT_NO_RESPONSE, this);
72         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
73         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
74         
75         /* Object */
76         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this);
77         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
78         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
79         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
80         
81         /* Fh Object */
82         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
83         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
84         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
85         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
86         
87         /* Object (Fix Pass) */
88         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
89         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::fix, DT_BROAD_RESPONSE, this);
90         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::fix, DT_BROAD_RESPONSE, this);
91         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
92 }
93
94 void SM_Scene::addTouchCallback(int response_class, DT_ResponseCallback callback, void *user)
95 {
96         DT_AddClassResponse(m_secondaryRespTable, m_secondaryResponseClass[response_class], callback, DT_BROAD_RESPONSE, user);
97 }
98
99 void SM_Scene::addSensor(SM_Object& object) 
100 {
101         T_ObjectList::iterator i =
102                 std::find(m_objectList.begin(), m_objectList.end(), &object);
103         if (i == m_objectList.end())
104         {
105                 object.calcXform();
106                 m_objectList.push_back(&object);
107                 DT_AddObject(m_scene, object.getObjectHandle());
108                 DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[SENSOR_RESPONSE]);
109                 DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass    [SENSOR_RESPONSE]);
110                 DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[SENSOR_RESPONSE]);
111         }
112 }
113
114 void SM_Scene::add(SM_Object& object) {
115         object.calcXform();
116         m_objectList.push_back(&object);
117         DT_AddObject(m_scene, object.getObjectHandle());
118         if (object.isDynamic()) {
119                 DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]);
120                 DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]);
121                 DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]);
122         } else {
123                 DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[STATIC_RESPONSE]);
124                 DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[STATIC_RESPONSE]);
125                 DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[STATIC_RESPONSE]);
126         }       
127
128         SM_FhObject *fh_object = object.getFhObject();
129
130         if (fh_object) {
131                 DT_AddObject(m_scene, fh_object->getObjectHandle());
132                 DT_SetResponseClass(m_respTable, fh_object->getObjectHandle(), m_ResponseClass[FH_RESPONSE]);
133                 DT_SetResponseClass(m_secondaryRespTable, fh_object->getObjectHandle(), m_secondaryResponseClass[FH_RESPONSE]);
134                 DT_SetResponseClass(m_fixRespTable, fh_object->getObjectHandle(), m_fixResponseClass[FH_RESPONSE]);
135         }
136 }       
137
138 void SM_Scene::requestCollisionCallback(SM_Object &object)
139 {
140         DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]);
141         DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]);
142 //      DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]);
143 }
144
145 void SM_Scene::remove(SM_Object& object) {
146         //std::cout << "SM_Scene::remove this =" << this << "object = " << &object << std::endl;
147         T_ObjectList::iterator i =
148                 std::find(m_objectList.begin(), m_objectList.end(), &object);
149         if (!(i == m_objectList.end()))
150         {
151                 std::swap(*i, m_objectList.back());
152                 m_objectList.pop_back();
153                 DT_RemoveObject(m_scene, object.getObjectHandle());
154
155                 SM_FhObject *fh_object = object.getFhObject();
156                 
157                 if (fh_object) {
158                         DT_RemoveObject(m_scene, fh_object->getObjectHandle());
159                 }
160         } 
161         else {
162                 // tried to remove an object that is not in the scene
163                 //assert(false);
164         }
165 }
166
167 void SM_Scene::beginFrame()
168 {
169         T_ObjectList::iterator i;
170         // Apply a forcefield (such as gravity)
171         for (i = m_objectList.begin(); i != m_objectList.end(); ++i)
172                 (*i)->applyForceField(m_forceField);
173
174 }
175
176 void SM_Scene::endFrame()
177 {
178         T_ObjectList::iterator i;
179         for (i = m_objectList.begin(); i != m_objectList.end(); ++i)
180                 (*i)->clearForce();
181 }
182
183 bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) 
184 {
185         if (!m_frames)
186         {
187                 if (ticrate > 0.)
188                         m_frames = (unsigned int)(curtime*ticrate) + 1.0;
189                 else
190                         m_frames = (unsigned int)(curtime*65536.0);
191         }
192         
193         // Divide the timeStep into a number of subsamples of size roughly 
194         // equal to subS (might be a little smaller).
195         MT_Scalar subStep;
196         int num_samples;
197         int frames = m_frames;
198         
199         // Compute the number of steps to do this update.
200         if (ticrate > 0.0)
201         {
202                 // Fixed time step
203                 subStep = 1.0/ticrate;
204                 num_samples = (unsigned int)(curtime*ticrate + 1.0) - m_frames;
205         
206                 if (num_samples > 4)
207                 {
208                         std::cout << "Dropping physics frames! frames:" << num_samples << " substep: " << subStep << std::endl;
209                         MT_Scalar tr = ticrate;
210                         do
211                         {
212                                 frames = frames / 2;
213                                 tr = tr / 2.0;
214                                 num_samples = (unsigned int)(curtime*tr + 1.0) - frames;
215                                 subStep *= 2.0;
216                         } while (num_samples > 8);
217                         std::cout << "                         frames:" << num_samples << " substep: " << subStep << std::endl;
218                 }
219         } 
220         else
221         {
222                 // Variable time step. (old update)
223                 // Integrate at least 100 Hz
224                 MT_Scalar timeStep = curtime - m_frames/65536.0;
225                 subStep = timeStep > 0.01 ? 0.01 : timeStep;
226                 num_samples = int(timeStep * 0.01);
227                 if (num_samples < 1)
228                         num_samples = 1;
229         }
230         
231         // Do a physics timestep.
232         T_ObjectList::iterator i;
233         if (num_samples > 0)
234         {
235                 // Do the integration steps per object.
236                 for (int step = 0; step != num_samples; ++step) 
237                 {
238                         MT_Scalar time;
239                         if (ticrate > 0.)
240                                 time = MT_Scalar(frames + step + 1) * subStep;
241                         else
242                                 time = MT_Scalar(m_frames)/65536.0 + MT_Scalar(step + 1)*subStep;
243                         
244                         for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
245                                 (*i)->endFrame();
246                                 // Apply a forcefield (such as gravity)
247                                 (*i)->integrateForces(subStep);
248                                 // And second we update the object positions by performing
249                                 // an integration step for each object
250                                 (*i)->integrateMomentum(subStep);
251                         }
252         
253                         // So now first we let the physics scene respond to 
254                         // new forces, velocities set externally. 
255                         // The collsion and friction impulses are computed here. 
256                         // Collision phase
257                         DT_Test(m_scene, m_respTable);
258                 
259                         // Contact phase
260                         DT_Test(m_scene, m_fixRespTable);
261                         
262                         // Finish this timestep by saving al state information for the next
263                         // timestep and clearing the accumulated forces. 
264                         for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
265                                 (*i)->relax();
266                                 (*i)->proceedKinematic(subStep);
267                                 (*i)->saveReactionForce(subStep);
268                                 (*i)->getNextFrame().setTime(time);
269                                 //(*i)->clearForce();
270                         }
271                 }
272         }
273
274         if (ticrate > 0)
275         {
276                 // Interpolate between time steps.
277                 for (i = m_objectList.begin(); i != m_objectList.end(); ++i) 
278                         (*i)->interpolate(curtime);
279         
280         //only update the m_frames after an actual physics timestep
281                 if (num_samples)
282                 {
283                         m_frames = (unsigned int)(curtime*ticrate) + 1.0;
284                 }
285         }
286         else
287         {
288                 m_frames = (unsigned int)(curtime*65536.0);
289         }
290                 
291         return num_samples != 0;
292 }
293
294 void SM_Scene::notifyCollision(SM_Object *obj1, SM_Object *obj2)
295 {
296         // For each pair of object that collided, call the corresponding callback.
297         if (m_secondaryRespTable)
298                 DT_CallResponse(m_secondaryRespTable, obj1->getObjectHandle(), obj2->getObjectHandle(), 0);
299 }
300
301
302 SM_Object *SM_Scene::rayTest(void *ignore_client, 
303                                                          const MT_Point3& from, const MT_Point3& to, 
304                                                          MT_Point3& result, MT_Vector3& normal) const {
305 #ifdef SM_DEBUG_RAYCAST
306         std::cout << "ray: { " << from << " } - { " << to << " }" << std::endl; 
307 #endif
308  
309         DT_Vector3 n, dfrom, dto;
310         DT_Scalar param;
311         from.getValue(dfrom);
312         to.getValue(dto);
313         SM_Object *hit_object = (SM_Object *) 
314                 DT_RayCast(m_scene, ignore_client, dfrom, dto, 1., &param, n);
315
316         if (hit_object) {
317                 //result = hit_object->getWorldCoord(from + (to - from)*param);
318                 result = from + (to - from) * param;
319                 normal.setValue(n);
320 #ifdef SM_DEBUG_RAYCAST
321                 std::cout << "ray: { " << from << " } -> { " << to << " }: { " << result 
322                   << " } (" << param << "), normal = { " << normal << " }" << std::endl;
323 #endif
324         }
325
326         return hit_object;
327 }
328
329 void SM_Scene::clearObjectCombinedVelocities() {
330
331         T_ObjectList::iterator i;
332
333         for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
334
335                 (*i)->clearCombinedVelocities();
336
337         }
338
339 }       
340
341
342 void SM_Scene::setSecondaryRespTable(DT_RespTableHandle secondaryRespTable) {
343         m_secondaryRespTable = secondaryRespTable;
344 }
345
346
347 DT_Bool SM_Scene::boing(
348         void *client_data,  
349         void *object1,
350         void *object2,
351         const DT_CollData *
352 ){
353         SM_Scene  *scene = (SM_Scene *)client_data; 
354         SM_Object *obj1  = (SM_Object *)object1;  
355         SM_Object *obj2  = (SM_Object *)object2;  
356         
357         scene->notifyCollision(obj1, obj2); // Record this collision for client callbacks
358
359 #ifdef SM_DEBUG_BOING   
360         printf("SM_Scene::boing\n");
361 #endif
362         
363         return DT_CONTINUE;
364 }
365
366 SM_Scene::~SM_Scene()
367
368         //std::cout << "SM_Scene::~ SM_Scene(): destroy " << this << std::endl;
369 //      if (m_objectList.begin() != m_objectList.end()) 
370 //              std::cout << "SM_Scene::~SM_Scene: There are still objects in the Sumo scene!" << std::endl;
371         for (T_ObjectList::iterator it = m_objectList.begin() ; it != m_objectList.end() ; it++)
372                 delete *it;
373         
374         DT_DestroyRespTable(m_respTable);
375         DT_DestroyRespTable(m_secondaryRespTable);
376         DT_DestroyRespTable(m_fixRespTable);
377         DT_DestroyScene(m_scene);
378 }