synched with trunk at revision 30597
[blender.git] / source / gameengine / Ketsji / KX_ObstacleSimulation.cpp
1 /**
2 * Simulation for obstacle avoidance behavior
3 *
4 * $Id$
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. 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 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 LICENSE BLOCK *****
33 */
34
35 #include "KX_ObstacleSimulation.h"
36 #include "KX_NavMeshObject.h"
37 #include "KX_PythonInit.h"
38 #include "DNA_object_types.h"
39 #include "BLI_math.h"
40
41 inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); }
42 inline float lerp(float a, float b, float t) { return a + (b-a)*t; }
43
44 static int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
45                                           const MT_Vector3& pos1, const MT_Scalar r1,
46                                           float& tmin, float& tmax)
47 {
48         static const float EPS = 0.0001f;
49         MT_Vector2 c0(pos0.x(), pos0.y());
50         MT_Vector2 c1(pos1.x(), pos1.y());
51         MT_Vector2 s = c1 - c0;
52         MT_Scalar  r = r0+r1;
53         float c = s.length2() - r*r;
54         float a = v.length2();
55         if (a < EPS) return 0;  // not moving
56
57         // Overlap, calc time to exit.
58         float b = MT_dot(v,s);
59         float d = b*b - a*c;
60         if (d < 0.0f) return 0; // no intersection.
61         tmin = (b - sqrtf(d)) / a;
62         tmax = (b + sqrtf(d)) / a;
63         return 1;
64 }
65
66 static int sweepCircleSegment(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
67                                            const MT_Vector3& pa, const MT_Vector3& pb, const MT_Scalar sr,
68                                            float& tmin, float &tmax)
69 {
70         // equation parameters
71         MT_Vector2 c0(pos0.x(), pos0.y());
72         MT_Vector2 sa(pa.x(), pa.y());
73         MT_Vector2 sb(pb.x(), pb.y());
74         MT_Vector2 L = sb-sa;
75         MT_Vector2 H = c0-sa;
76         MT_Scalar radius = r0+sr;
77         float l2 = L.length2();
78         float r2 = radius * radius;
79         float dl = perp(v, L);
80         float hl = perp(H, L);
81         float a = dl * dl;
82         float b = 2.0f * hl * dl;
83         float c = hl * hl - (r2 * l2);
84         float d = (b*b) - (4.0f * a * c);
85
86         // infinite line missed by infinite ray.
87         if (d < 0.0f)
88                 return 0;
89
90         d = sqrtf(d);
91         tmin = (-b - d) / (2.0f * a);
92         tmax = (-b + d) / (2.0f * a);
93
94         // line missed by ray range.
95         /*      if (tmax < 0.0f || tmin > 1.0f)
96         return 0;*/
97
98         // find what part of the ray was collided.
99         MT_Vector2 Pedge;
100         Pedge = c0+v*tmin;
101         H = Pedge - sa;
102         float e0 = MT_dot(H, L) / l2;
103         Pedge = c0 + v*tmax;
104         H = Pedge - sa;
105         float e1 = MT_dot(H, L) / l2;
106
107         if (e0 < 0.0f || e1 < 0.0f)
108         {
109                 float ctmin, ctmax;
110                 if (sweepCircleCircle(pos0, r0, v, pa, sr, ctmin, ctmax))
111                 {
112                         if (e0 < 0.0f && ctmin > tmin)
113                                 tmin = ctmin;
114                         if (e1 < 0.0f && ctmax < tmax)
115                                 tmax = ctmax;
116                 }
117                 else
118                 {
119                         return 0;
120                 }
121         }
122
123         if (e0 > 1.0f || e1 > 1.0f)
124         {
125                 float ctmin, ctmax;
126                 if (sweepCircleCircle(pos0, r0, v, pb, sr, ctmin, ctmax))
127                 {
128                         if (e0 > 1.0f && ctmin > tmin)
129                                 tmin = ctmin;
130                         if (e1 > 1.0f && ctmax < tmax)
131                                 tmax = ctmax;
132                 }
133                 else
134                 {
135                         return 0;
136                 }
137         }
138
139         return 1;
140 }
141
142 static bool inBetweenAngle(float a, float amin, float amax, float& t)
143 {
144         if (amax < amin) amax += (float)M_PI*2;
145         if (a < amin-(float)M_PI) a += (float)M_PI*2;
146         if (a > amin+(float)M_PI) a -= (float)M_PI*2;
147         if (a >= amin && a < amax)
148         {
149                 t = (a-amin) / (amax-amin);
150                 return true;
151         }
152         return false;
153 }
154
155 static float interpolateToi(float a, const float* dir, const float* toi, const int ntoi)
156 {
157         for (int i = 0; i < ntoi; ++i)
158         {
159                 int next = (i+1) % ntoi;
160                 float t;
161                 if (inBetweenAngle(a, dir[i], dir[next], t))
162                 {
163                         return lerp(toi[i], toi[next], t);
164                 }
165         }
166         return 0;
167 }
168
169 KX_ObstacleSimulation::KX_ObstacleSimulation(MT_Scalar levelHeight, bool enableVisualization)
170 :       m_levelHeight(levelHeight)
171 ,       m_enableVisualization(enableVisualization)
172 {
173
174 }
175
176 KX_ObstacleSimulation::~KX_ObstacleSimulation()
177 {
178         for (size_t i=0; i<m_obstacles.size(); i++)
179         {
180                 KX_Obstacle* obs = m_obstacles[i];
181                 delete obs;
182         }
183         m_obstacles.clear();
184 }
185 KX_Obstacle* KX_ObstacleSimulation::CreateObstacle(KX_GameObject* gameobj)
186 {
187         KX_Obstacle* obstacle = new KX_Obstacle();
188         obstacle->m_gameObj = gameobj;
189         gameobj->RegisterObstacle(this);
190         m_obstacles.push_back(obstacle);
191         return obstacle;
192 }
193
194 void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj)
195 {
196         KX_Obstacle* obstacle = CreateObstacle(gameobj);
197         struct Object* blenderobject = gameobj->GetBlenderObject();
198         obstacle->m_type = KX_OBSTACLE_OBJ;
199         obstacle->m_shape = KX_OBSTACLE_CIRCLE;
200         obstacle->m_rad = blenderobject->obstacleRad;
201 }
202
203 void KX_ObstacleSimulation::AddObstaclesForNavMesh(KX_NavMeshObject* navmeshobj)
204 {       
205         dtStatNavMesh* navmesh = navmeshobj->GetNavMesh();
206         if (navmesh)
207         {
208                 int npoly = navmesh->getPolyCount();
209                 for (int pi=0; pi<npoly; pi++)
210                 {
211                         const dtStatPoly* poly = navmesh->getPoly(pi);
212
213                         for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
214                         {       
215                                 if (poly->n[j]) continue;
216                                 const float* vj = navmesh->getVertex(poly->v[j]);
217                                 const float* vi = navmesh->getVertex(poly->v[i]);
218                 
219                                 KX_Obstacle* obstacle = CreateObstacle(navmeshobj);
220                                 obstacle->m_type = KX_OBSTACLE_NAV_MESH;
221                                 obstacle->m_shape = KX_OBSTACLE_SEGMENT;
222                                 obstacle->m_pos = MT_Point3(vj[0], vj[2], vj[1]);
223                                 obstacle->m_pos2 = MT_Point3(vi[0], vi[2], vi[1]);
224                                 obstacle->m_rad = 0;
225                                 obstacle->m_vel = MT_Vector2(0,0);
226                         }
227                 }
228         }
229 }
230
231 void KX_ObstacleSimulation::DestroyObstacleForObj(KX_GameObject* gameobj)
232 {
233         for (size_t i=0; i<m_obstacles.size(); )
234         {
235                 if (m_obstacles[i]->m_gameObj == gameobj)
236                 {
237                         KX_Obstacle* obstacle = m_obstacles[i];
238                         obstacle->m_gameObj->UnregisterObstacle();
239                         m_obstacles[i] = m_obstacles.back();
240                         m_obstacles.pop_back();
241                         delete obstacle;
242                 }
243                 else
244                         i++;
245         }
246 }
247
248 void KX_ObstacleSimulation::UpdateObstacles()
249 {
250         for (size_t i=0; i<m_obstacles.size(); i++)
251         {
252                 if (m_obstacles[i]->m_type==KX_OBSTACLE_NAV_MESH || m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
253                         continue;
254
255                 KX_Obstacle* obs = m_obstacles[i];
256                 obs->m_pos = obs->m_gameObj->NodeGetWorldPosition();
257                 obs->m_vel.x() = obs->m_gameObj->GetLinearVelocity().x();
258                 obs->m_vel.y() = obs->m_gameObj->GetLinearVelocity().y();
259         }
260 }
261
262 KX_Obstacle* KX_ObstacleSimulation::GetObstacle(KX_GameObject* gameobj)
263 {
264         for (size_t i=0; i<m_obstacles.size(); i++)
265         {
266                 if (m_obstacles[i]->m_gameObj == gameobj)
267                         return m_obstacles[i];
268         }
269
270         return NULL;
271 }
272
273 void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
274                                                                                 MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle)
275 {
276 }
277
278 void KX_ObstacleSimulation::DrawObstacles()
279 {
280         if (!m_enableVisualization)
281                 return;
282         static const MT_Vector3 bluecolor(0,0,1);
283         static const MT_Vector3 normal(0.,0.,1.);
284         static const int SECTORS_NUM = 32;
285         for (size_t i=0; i<m_obstacles.size(); i++)
286         {
287                 if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
288                 {
289                         MT_Point3 p1 = m_obstacles[i]->m_pos;
290                         MT_Point3 p2 = m_obstacles[i]->m_pos2;
291                         //apply world transform
292                         if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH)
293                         {
294                                 KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(m_obstacles[i]->m_gameObj);
295                                 p1 = navmeshobj->TransformToWorldCoords(p1);
296                                 p2 = navmeshobj->TransformToWorldCoords(p2);
297                         }
298
299                         KX_RasterizerDrawDebugLine(p1, p2, bluecolor);
300                 }
301                 else if (m_obstacles[i]->m_shape==KX_OBSTACLE_CIRCLE)
302                 {
303                         KX_RasterizerDrawDebugCircle(m_obstacles[i]->m_pos, m_obstacles[i]->m_rad, bluecolor,
304                                                                                 normal, SECTORS_NUM);
305                 }
306         }       
307 }
308
309 static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle)
310 {
311         switch (obstacle->m_shape)
312         {
313         case KX_OBSTACLE_SEGMENT :
314         {
315                 MT_Vector3 ab = obstacle->m_pos2 - obstacle->m_pos;
316                 if (!ab.fuzzyZero())
317                 {
318                         MT_Vector3 abdir = ab.normalized();
319                         MT_Vector3  v = pos - obstacle->m_pos;
320                         MT_Scalar proj = abdir.dot(v);
321                         CLAMP(proj, 0, ab.length());
322                         MT_Point3 res = obstacle->m_pos + abdir*proj;
323                         return res;
324                 }               
325         }
326         case KX_OBSTACLE_CIRCLE :
327         default:
328                 return obstacle->m_pos;
329         }
330 }
331
332 bool KX_ObstacleSimulation::FilterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst)
333 {
334         //filter obstacles by type
335         if ( (otherObst == activeObst) ||
336                 (otherObst->m_type==KX_OBSTACLE_NAV_MESH && otherObst->m_gameObj!=activeNavMeshObj)     )
337                 return false;
338
339         //filter obstacles by position
340         MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst);
341         if ( fabs(activeObst->m_pos.z() - p.z()) > m_levelHeight)
342                 return false;
343
344         return true;
345 }
346
347 KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI(MT_Scalar levelHeight, bool enableVisualization):
348         KX_ObstacleSimulation(levelHeight, enableVisualization),
349         m_avoidSteps(32),
350         m_minToi(0.5f),
351         m_maxToi(1.2f),
352         m_angleWeight(4.0f),
353         m_toiWeight(1.0f),
354         m_collisionWeight(100.0f)
355 {
356         
357 }
358
359 KX_ObstacleSimulationTOI::~KX_ObstacleSimulationTOI()
360 {
361         for (size_t i=0; i<m_toiCircles.size(); i++)
362         {
363                 TOICircle* toi = m_toiCircles[i];
364                 delete toi;
365         }
366         m_toiCircles.clear();
367 }
368
369 KX_Obstacle* KX_ObstacleSimulationTOI::CreateObstacle(KX_GameObject* gameobj)
370 {
371         KX_Obstacle* obstacle = KX_ObstacleSimulation::CreateObstacle(gameobj);
372         m_toiCircles.push_back(new TOICircle());
373         return obstacle;
374 }
375
376 void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
377                                                                                 MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle)
378 {
379         int nobs = m_obstacles.size();
380         int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
381         if (obstidx == nobs)
382                 return; 
383         TOICircle* tc = m_toiCircles[obstidx];
384
385         MT_Vector2 vel(velocity.x(), velocity.y());
386         float vmax = (float) velocity.length();
387         float odir = (float) atan2(velocity.y(), velocity.x());
388
389         MT_Vector2 ddir = vel;
390         ddir.normalize();
391
392         float bestScore = FLT_MAX;
393         float bestDir = odir;
394         float bestToi = 0;
395
396         tc->n = m_avoidSteps;
397         tc->minToi = m_minToi;
398         tc->maxToi = m_maxToi;
399
400         const int iforw = m_avoidSteps/2;
401         const float aoff = (float)iforw / (float)m_avoidSteps;
402
403         for (int iter = 0; iter < m_avoidSteps; ++iter)
404         {
405                 // Calculate sample velocity
406                 const float ndir = ((float)iter/(float)m_avoidSteps) - aoff;
407                 const float dir = odir+ndir*M_PI*2;
408                 MT_Vector2 svel;
409                 svel.x() = cosf(dir) * vmax;
410                 svel.y() = sinf(dir) * vmax;
411
412                 // Find min time of impact and exit amongst all obstacles.
413                 float tmin = m_maxToi;
414                 float tmine = 0;
415                 for (int i = 0; i < nobs; ++i)
416                 {
417                         KX_Obstacle* ob = m_obstacles[i];
418                         bool res = FilterObstacle(activeObst, activeNavMeshObj, ob);
419                         if (!res)
420                                 continue;
421                         
422                         float htmin,htmax;
423
424                         if (ob->m_shape == KX_OBSTACLE_CIRCLE)
425                         {
426                                 MT_Vector2 vab;
427                                 if (ob->m_vel.length2() < 0.01f*0.01f)
428                                 {
429                                         // Stationary, use VO
430                                         vab = svel;
431                                 }
432                                 else
433                                 {
434                                         // Moving, use RVO
435                                         vab = 2*svel - vel - ob->m_vel;
436                                 }
437
438                                 if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, 
439                                                                                 vab, ob->m_pos, ob->m_rad, htmin, htmax))
440                                         continue;
441                         }
442                         else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
443                         {
444                                 MT_Point3 p1 = ob->m_pos;
445                                 MT_Point3 p2 = ob->m_pos2;
446                                 //apply world transform
447                                 if (ob->m_type == KX_OBSTACLE_NAV_MESH)
448                                 {
449                                         KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
450                                         p1 = navmeshobj->TransformToWorldCoords(p1);
451                                         p2 = navmeshobj->TransformToWorldCoords(p2);
452                                 }
453                                 if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel, 
454                                                                                 p1, p2, ob->m_rad, htmin, htmax))
455                                         continue;
456                         }
457
458                         if (htmin > 0.0f)
459                         {
460                                 // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
461                                 if (htmin < tmin)
462                                         tmin = htmin;
463                         }
464                         else if (htmax > 0.0f)
465                         {
466                                 // The agent overlaps the obstacle, keep track of first safe exit.
467                                 if (htmax > tmine)
468                                         tmine = htmax;
469                         }
470                 }
471
472                 // Calculate sample penalties and final score.
473                 const float apen = m_angleWeight * fabsf(ndir);
474                 const float tpen = m_toiWeight * (1.0f/(0.0001f+tmin/m_maxToi));
475                 const float cpen = m_collisionWeight * (tmine/m_minToi)*(tmine/m_minToi);
476                 const float score = apen + tpen + cpen;
477
478                 // Update best score.
479                 if (score < bestScore)
480                 {
481                         bestDir = dir;
482                         bestToi = tmin;
483                         bestScore = score;
484                 }
485
486                 tc->dir[iter] = dir;
487                 tc->toi[iter] = tmin;
488                 tc->toie[iter] = tmine;
489         }
490
491         if (activeObst->m_vel.length() > 0.1)
492         {
493                 // Constrain max turn rate.
494                 float cura = atan2(activeObst->m_vel.y(),activeObst->m_vel.x());
495                 float da = bestDir - cura;
496                 if (da < -M_PI) da += (float)M_PI*2;
497                 if (da > M_PI) da -= (float)M_PI*2;
498                 if (da < -maxDeltaAngle)
499                 {
500                         bestDir = cura - maxDeltaAngle;
501                         bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n));
502                 }
503                 else if (da > maxDeltaAngle)
504                 {
505                         bestDir = cura + maxDeltaAngle;
506                         bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n));
507                 }
508         }
509
510         // Adjust speed when time of impact is less than min TOI.
511         if (bestToi < m_minToi)
512                 vmax *= bestToi/m_minToi;
513
514         // Constrain velocity change.
515         const float curSpeed = (float) activeObst->m_vel.length();
516         float deltaSpeed =  vmax - curSpeed; 
517         CLAMP(deltaSpeed, -maxDeltaSpeed, maxDeltaSpeed);
518         vmax = curSpeed + deltaSpeed;
519
520         // New steering velocity.
521         vel.x() = cosf(bestDir) * vmax;
522         vel.y() = sinf(bestDir) * vmax;
523
524         velocity.x() = vel.x();
525         velocity.y() = vel.y();
526 }