Speed up the physics engine: hook the SOLID broad phase, so we can either reject...
[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         m_lastTime(-1.0)
58 {
59         for (int i = 0 ; i < NUM_RESPONSE; i++)
60         {
61                 m_ResponseClass[i] = DT_GenResponseClass(m_respTable);
62                 m_secondaryResponseClass[i] = DT_GenResponseClass(m_secondaryRespTable);
63                 m_fixResponseClass[i] = DT_GenResponseClass(m_fixRespTable);
64         }
65         
66         /* Sensor */
67         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
68         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
69         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
70         DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
71         
72         /* Static */
73         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
74         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[STATIC_RESPONSE], 0, DT_NO_RESPONSE, this);
75         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
76         DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
77         
78         /* Object */
79         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_BROAD_RESPONSE, this);
80         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
81         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_BROAD_RESPONSE, this);
82         DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
83         
84         /* Fh Object */
85         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
86         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
87         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
88         DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
89         
90         /* Object (Fix Pass) */
91         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
92         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::fix, DT_BROAD_RESPONSE, this);
93         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::fix, DT_BROAD_RESPONSE, this);
94         DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
95 }
96
97 void SM_Scene::addTouchCallback(int response_class, DT_ResponseCallback callback, void *user)
98 {
99         DT_AddClassResponse(m_secondaryRespTable, m_secondaryResponseClass[response_class], callback, DT_BROAD_RESPONSE, user);
100 }
101
102 void SM_Scene::addSensor(SM_Object& object) 
103 {
104         object.calcXform();
105         m_objectList.push_back(&object);
106         DT_AddObject(m_scene, object.getObjectHandle());
107         DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[SENSOR_RESPONSE]);
108         DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[SENSOR_RESPONSE]);
109         DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[SENSOR_RESPONSE]);
110 }
111
112 void SM_Scene::add(SM_Object& object) {
113         object.calcXform();
114         m_objectList.push_back(&object);
115         DT_AddObject(m_scene, object.getObjectHandle());
116         if (object.isDynamic()) {
117                 DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]);
118                 DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]);
119                 DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]);
120         } else {
121                 DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[STATIC_RESPONSE]);
122                 DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[STATIC_RESPONSE]);
123                 DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[STATIC_RESPONSE]);
124         }       
125
126         SM_FhObject *fh_object = object.getFhObject();
127
128         if (fh_object) {
129                 DT_AddObject(m_scene, fh_object->getObjectHandle());
130                 DT_SetResponseClass(m_respTable, fh_object->getObjectHandle(), m_ResponseClass[FH_RESPONSE]);
131                 DT_SetResponseClass(m_secondaryRespTable, fh_object->getObjectHandle(), m_secondaryResponseClass[FH_RESPONSE]);
132                 DT_SetResponseClass(m_fixRespTable, fh_object->getObjectHandle(), m_fixResponseClass[FH_RESPONSE]);
133         }
134 }       
135
136 void SM_Scene::requestCollisionCallback(SM_Object &object)
137 {
138         DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]);
139         DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]);
140 //      DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]);
141 }
142
143 void SM_Scene::remove(SM_Object& object) {      
144         T_ObjectList::iterator i =
145                 std::find(m_objectList.begin(), m_objectList.end(), &object);
146         if (!(i == m_objectList.end()))
147         {
148                 std::swap(*i, m_objectList.back());
149                 m_objectList.pop_back();
150                 DT_RemoveObject(m_scene, object.getObjectHandle());
151
152                 SM_FhObject *fh_object = object.getFhObject();
153                 
154                 if (fh_object) {
155                         DT_RemoveObject(m_scene, fh_object->getObjectHandle());
156                 }
157         } 
158         else {
159                 // tried to remove an object that is not in the scene
160                 //assert(false);
161         }
162 }
163
164 void SM_Scene::beginFrame()
165 {
166         T_ObjectList::iterator i;
167         // Apply a forcefield (such as gravity)
168         for (i = m_objectList.begin(); i != m_objectList.end(); ++i)
169                 (*i)->applyForceField(m_forceField);
170
171 }
172
173 void SM_Scene::endFrame()
174 {
175         T_ObjectList::iterator i;
176         for (i = m_objectList.begin(); i != m_objectList.end(); ++i)
177                 (*i)->clearForce();
178 }
179
180 bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
181         if (m_lastTime < 0.0)
182         {
183                 m_lastTime = curtime;
184                 return false;
185         }
186                 
187         // Divide the timeStep into a number of subsamples of size roughly 
188         // equal to subS (might be a little smaller).
189         MT_Scalar timeStep = curtime - m_lastTime;
190         MT_Scalar subStep = 1.0/ticrate;
191         int num_samples = int(timeStep * ticrate);
192         
193         T_ObjectList::iterator i;
194         
195         // No timestep! (should do a mini update)
196         if (num_samples <= 0)
197         {
198                 // Apply a forcefield (such as gravity)
199 #if 0
200                 for (i = m_objectList.begin(); i != m_objectList.end(); ++i) 
201                 {
202                         //(*i)->applyForceField(m_forceField);
203                         //(*i)->integrateForces(timeStep);
204                         // And second we update the object positions by performing
205                         // an integration step for each object
206                         (*i)->integrateMomentum(timeStep);
207                         //(*i)->clearForce();
208                 }
209 #endif
210                 return false;   
211         }
212
213         m_lastTime += MT_Scalar(num_samples)*subStep;
214         
215         // Do the integration steps per object.
216         int step;
217         for (step = 0; step != num_samples; ++step) {
218
219                 for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
220                         // Apply a forcefield (such as gravity)
221                         //(*i)->applyForceField(m_forceField);
222                         //(*i)->setTimeStep(timeStep);
223                         (*i)->integrateForces(subStep);
224                         // And second we update the object positions by performing
225                         // an integration step for each object
226                         (*i)->integrateMomentum(subStep);
227                 }
228                 
229                 // I changed the order of the next 2 statements.
230                 // Originally objects were first integrated with a call
231                 // to proceed(). However if external objects were 
232                 // directly manipulating the velocities etc of physics 
233                 // objects then the physics environment would not be able 
234                 // to react before object positions were updated. --- Laurence.
235
236                 // So now first we let the physics scene respond to 
237                 // new forces, velocities set externally. 
238                 // The collsion and friction impulses are computed here. 
239                 DT_Test(m_scene, m_respTable);
240
241         // clear the user set velocities.
242 #if 0
243         clearObjectCombinedVelocities();
244 #endif
245                 DT_Test(m_scene, m_fixRespTable);
246                 
247                 // Finish this timestep by saving al state information for the next
248                 // timestep and clearing the accumulated forces. 
249                 for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
250                         (*i)->relax();
251                         (*i)->proceedKinematic(subStep);
252                         (*i)->saveReactionForce(subStep);
253                         //(*i)->clearForce();
254                 }
255         }
256         // For each pair of object that collided, call the corresponding callback.
257         // Additional collisions of a pair within the same time step are ignored.
258
259         if (m_secondaryRespTable) {
260                 T_PairList::iterator p;
261                 for (p = m_pairList.begin(); p != m_pairList.end(); ++p) {
262                         DT_CallResponse(m_secondaryRespTable, 
263                                                         (*p).first->getObjectHandle(), 
264                                                         (*p).second->getObjectHandle(), 
265                                                         0);
266                 }
267         }
268         
269         clearPairs();
270         
271         return true;
272 }
273
274 SM_Object *SM_Scene::rayTest(void *ignore_client, 
275                                                          const MT_Point3& from, const MT_Point3& to, 
276                                                          MT_Point3& result, MT_Vector3& normal) const {
277 #ifdef SM_DEBUG_RAYCAST
278         std::cout << "ray: { " << from << " } - { " << to << " }" << std::endl; 
279 #endif
280  
281         DT_Vector3 n, dfrom, dto;
282         DT_Scalar param;
283         from.getValue(dfrom);
284         to.getValue(dto);
285         SM_Object *hit_object = (SM_Object *) 
286                 DT_RayCast(m_scene, ignore_client, dfrom, dto, 1., &param, n);
287
288         if (hit_object) {
289                 //result = hit_object->getWorldCoord(from + (to - from)*param);
290                 result = from + (to - from) * param;
291                 normal.setValue(n);
292 #ifdef SM_DEBUG_RAYCAST
293                 std::cout << "ray: { " << from << " } -> { " << to << " }: { " << result 
294                   << " } (" << param << "), normal = { " << normal << " }" << std::endl;
295 #endif
296         }
297
298         return hit_object;
299 }
300
301 void SM_Scene::clearObjectCombinedVelocities() {
302
303         T_ObjectList::iterator i;
304
305         for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
306
307                 (*i)->clearCombinedVelocities();
308
309         }
310
311 }       
312
313
314 void SM_Scene::setSecondaryRespTable(DT_RespTableHandle secondaryRespTable) {
315         m_secondaryRespTable = secondaryRespTable;
316 }
317
318
319 DT_Bool SM_Scene::boing(
320         void *client_data,  
321         void *object1,
322         void *object2,
323         const DT_CollData *coll_data
324 ){
325         SM_Scene  *scene = (SM_Scene *)client_data; 
326         SM_Object *obj1  = (SM_Object *)object1;  
327         SM_Object *obj2  = (SM_Object *)object2;  
328         
329         scene->addPair(obj1, obj2); // Record this collision for client callbacks
330
331 #ifdef SM_DEBUG_BOING   
332         printf("SM_Scene::boing\n");
333 #endif
334         
335         return DT_CONTINUE;
336 }
337
338 SM_Scene::~SM_Scene()
339
340 //      if (m_objectList.begin() != m_objectList.end()) 
341 //              std::cout << "SM_Scene::~SM_Scene: There are still objects in the Sumo scene!" << std::endl;
342         for (T_ObjectList::iterator it = m_objectList.begin() ; it != m_objectList.end() ; it++)
343                 delete *it;
344         
345         DT_DestroyRespTable(m_respTable);
346         DT_DestroyRespTable(m_secondaryRespTable);
347         DT_DestroyRespTable(m_fixRespTable);
348         DT_DestroyScene(m_scene);
349 }