Work on conversion of the navigation mesh: we build navmesh directly from blender...
[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)
170 :       m_levelHeight(levelHeight)
171 {
172
173 }
174
175 KX_ObstacleSimulation::~KX_ObstacleSimulation()
176 {
177         for (size_t i=0; i<m_obstacles.size(); i++)
178         {
179                 KX_Obstacle* obs = m_obstacles[i];
180                 delete obs;
181         }
182         m_obstacles.clear();
183 }
184 KX_Obstacle* KX_ObstacleSimulation::CreateObstacle(KX_GameObject* gameobj)
185 {
186         KX_Obstacle* obstacle = new KX_Obstacle();
187         obstacle->m_gameObj = gameobj;
188         gameobj->RegisterObstacle(this);
189         m_obstacles.push_back(obstacle);
190         return obstacle;
191 }
192
193 void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj)
194 {
195         KX_Obstacle* obstacle = CreateObstacle(gameobj);
196         struct Object* blenderobject = gameobj->GetBlenderObject();
197         obstacle->m_type = KX_OBSTACLE_OBJ;
198         obstacle->m_shape = KX_OBSTACLE_CIRCLE;
199         obstacle->m_rad = blenderobject->obstacleRad;
200 }
201
202 void KX_ObstacleSimulation::AddObstaclesForNavMesh(KX_NavMeshObject* navmeshobj)
203 {       
204         dtStatNavMesh* navmesh = navmeshobj->GetNavMesh();
205         if (navmesh)
206         {
207                 int npoly = navmesh->getPolyCount();
208                 for (int pi=0; pi<npoly; pi++)
209                 {
210                         const dtStatPoly* poly = navmesh->getPoly(pi);
211
212                         for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
213                         {       
214                                 if (poly->n[j]) continue;
215                                 const float* vj = navmesh->getVertex(poly->v[j]);
216                                 const float* vi = navmesh->getVertex(poly->v[i]);
217                 
218                                 KX_Obstacle* obstacle = CreateObstacle(navmeshobj);
219                                 obstacle->m_type = KX_OBSTACLE_NAV_MESH;
220                                 obstacle->m_shape = KX_OBSTACLE_SEGMENT;
221                                 obstacle->m_pos = MT_Point3(vj[0], vj[2], vj[1]);
222                                 obstacle->m_pos2 = MT_Point3(vi[0], vi[2], vi[1]);
223                                 obstacle->m_rad = 0;
224                                 obstacle->m_vel = MT_Vector2(0,0);
225                         }
226                 }
227         }
228 }
229
230 void KX_ObstacleSimulation::DestroyObstacleForObj(KX_GameObject* gameobj)
231 {
232         for (size_t i=0; i<m_obstacles.size(); )
233         {
234                 if (m_obstacles[i]->m_gameObj == gameobj)
235                 {
236                         KX_Obstacle* obstacle = m_obstacles[i];
237                         obstacle->m_gameObj->UnregisterObstacle();
238                         m_obstacles[i] = m_obstacles.back();
239                         m_obstacles.pop_back();
240                         delete obstacle;
241                 }
242                 else
243                         i++;
244         }
245 }
246
247 void KX_ObstacleSimulation::UpdateObstacles()
248 {
249         for (size_t i=0; i<m_obstacles.size(); i++)
250         {
251                 if (m_obstacles[i]->m_type==KX_OBSTACLE_NAV_MESH || m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
252                         continue;
253
254                 KX_Obstacle* obs = m_obstacles[i];
255                 obs->m_pos = obs->m_gameObj->NodeGetWorldPosition();
256                 obs->m_vel.x() = obs->m_gameObj->GetLinearVelocity().x();
257                 obs->m_vel.y() = obs->m_gameObj->GetLinearVelocity().y();
258         }
259 }
260
261 KX_Obstacle* KX_ObstacleSimulation::GetObstacle(KX_GameObject* gameobj)
262 {
263         for (size_t i=0; i<m_obstacles.size(); i++)
264         {
265                 if (m_obstacles[i]->m_gameObj == gameobj)
266                         return m_obstacles[i];
267         }
268
269         return NULL;
270 }
271
272 void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
273                                                                                 MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle)
274 {
275 }
276
277 void KX_ObstacleSimulation::DrawObstacles()
278 {
279         static const MT_Vector3 bluecolor(0,0,1);
280         static const MT_Vector3 normal(0.,0.,1.);
281         static const int SECTORS_NUM = 32;
282         for (size_t i=0; i<m_obstacles.size(); i++)
283         {
284                 if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
285                 {
286                         MT_Point3 p1 = m_obstacles[i]->m_pos;
287                         MT_Point3 p2 = m_obstacles[i]->m_pos2;
288                         //apply world transform
289                         if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH)
290                         {
291                                 KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(m_obstacles[i]->m_gameObj);
292                                 p1 = navmeshobj->TransformToWorldCoords(p1);
293                                 p2 = navmeshobj->TransformToWorldCoords(p2);
294                         }
295
296                         KX_RasterizerDrawDebugLine(p1, p2, bluecolor);
297                 }
298                 else if (m_obstacles[i]->m_shape==KX_OBSTACLE_CIRCLE)
299                 {
300                         KX_RasterizerDrawDebugCircle(m_obstacles[i]->m_pos, m_obstacles[i]->m_rad, bluecolor,
301                                                                                 normal, SECTORS_NUM);
302                 }
303         }       
304 }
305
306 static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle)
307 {
308         switch (obstacle->m_shape)
309         {
310         case KX_OBSTACLE_SEGMENT :
311         {
312                 MT_Vector3 ab = obstacle->m_pos2 - obstacle->m_pos;
313                 if (!ab.fuzzyZero())
314                 {
315                         MT_Vector3 abdir = ab.normalized();
316                         MT_Vector3  v = pos - obstacle->m_pos;
317                         MT_Scalar proj = abdir.dot(v);
318                         CLAMP(proj, 0, ab.length());
319                         MT_Point3 res = obstacle->m_pos + abdir*proj;
320                         return res;
321                 }               
322         }
323         case KX_OBSTACLE_CIRCLE :
324         default:
325                 return obstacle->m_pos;
326         }
327 }
328
329 bool KX_ObstacleSimulation::FilterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst)
330 {
331         //filter obstacles by type
332         if ( (otherObst == activeObst) ||
333                 (otherObst->m_type==KX_OBSTACLE_NAV_MESH && otherObst->m_gameObj!=activeNavMeshObj)     )
334                 return false;
335
336         //filter obstacles by position
337         MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst);
338         if ( fabs(activeObst->m_pos.z() - p.z()) > m_levelHeight)
339                 return false;
340
341         return true;
342 }
343
344 KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI(MT_Scalar levelHeight):
345         KX_ObstacleSimulation(levelHeight),
346         m_avoidSteps(32),
347         m_minToi(0.5f),
348         m_maxToi(1.2f),
349         m_angleWeight(4.0f),
350         m_toiWeight(1.0f),
351         m_collisionWeight(100.0f)
352 {
353         
354 }
355
356 KX_ObstacleSimulationTOI::~KX_ObstacleSimulationTOI()
357 {
358         for (size_t i=0; i<m_toiCircles.size(); i++)
359         {
360                 TOICircle* toi = m_toiCircles[i];
361                 delete toi;
362         }
363         m_toiCircles.clear();
364 }
365
366 KX_Obstacle* KX_ObstacleSimulationTOI::CreateObstacle(KX_GameObject* gameobj)
367 {
368         KX_Obstacle* obstacle = KX_ObstacleSimulation::CreateObstacle(gameobj);
369         m_toiCircles.push_back(new TOICircle());
370         return obstacle;
371 }
372
373 void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
374                                                                                 MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle)
375 {
376         int nobs = m_obstacles.size();
377         int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
378         if (obstidx == nobs)
379                 return; 
380         TOICircle* tc = m_toiCircles[obstidx];
381
382         MT_Vector2 vel(velocity.x(), velocity.y());
383         float vmax = (float) velocity.length();
384         float odir = (float) atan2(velocity.y(), velocity.x());
385
386         MT_Vector2 ddir = vel;
387         ddir.normalize();
388
389         float bestScore = FLT_MAX;
390         float bestDir = odir;
391         float bestToi = 0;
392
393         tc->n = m_avoidSteps;
394         tc->minToi = m_minToi;
395         tc->maxToi = m_maxToi;
396
397         const int iforw = m_avoidSteps/2;
398         const float aoff = (float)iforw / (float)m_avoidSteps;
399
400         for (int iter = 0; iter < m_avoidSteps; ++iter)
401         {
402                 // Calculate sample velocity
403                 const float ndir = ((float)iter/(float)m_avoidSteps) - aoff;
404                 const float dir = odir+ndir*M_PI*2;
405                 MT_Vector2 svel;
406                 svel.x() = cosf(dir) * vmax;
407                 svel.y() = sinf(dir) * vmax;
408
409                 // Find min time of impact and exit amongst all obstacles.
410                 float tmin = m_maxToi;
411                 float tmine = 0;
412                 for (int i = 0; i < nobs; ++i)
413                 {
414                         KX_Obstacle* ob = m_obstacles[i];
415                         bool res = FilterObstacle(activeObst, activeNavMeshObj, ob);
416                         if (!res)
417                                 continue;
418                         
419                         float htmin,htmax;
420
421                         if (ob->m_shape == KX_OBSTACLE_CIRCLE)
422                         {
423                                 MT_Vector2 vab;
424                                 if (ob->m_vel.length2() < 0.01f*0.01f)
425                                 {
426                                         // Stationary, use VO
427                                         vab = svel;
428                                 }
429                                 else
430                                 {
431                                         // Moving, use RVO
432                                         vab = 2*svel - vel - ob->m_vel;
433                                 }
434
435                                 if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, 
436                                                                                 vab, ob->m_pos, ob->m_rad, htmin, htmax))
437                                         continue;
438                         }
439                         else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
440                         {
441                                 MT_Point3 p1 = ob->m_pos;
442                                 MT_Point3 p2 = ob->m_pos2;
443                                 //apply world transform
444                                 if (ob->m_type == KX_OBSTACLE_NAV_MESH)
445                                 {
446                                         KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
447                                         p1 = navmeshobj->TransformToWorldCoords(p1);
448                                         p2 = navmeshobj->TransformToWorldCoords(p2);
449                                 }
450                                 if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel, 
451                                                                                 p1, p2, ob->m_rad, htmin, htmax))
452                                         continue;
453                         }
454
455                         if (htmin > 0.0f)
456                         {
457                                 // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
458                                 if (htmin < tmin)
459                                         tmin = htmin;
460                         }
461                         else if (htmax > 0.0f)
462                         {
463                                 // The agent overlaps the obstacle, keep track of first safe exit.
464                                 if (htmax > tmine)
465                                         tmine = htmax;
466                         }
467                 }
468
469                 // Calculate sample penalties and final score.
470                 const float apen = m_angleWeight * fabsf(ndir);
471                 const float tpen = m_toiWeight * (1.0f/(0.0001f+tmin/m_maxToi));
472                 const float cpen = m_collisionWeight * (tmine/m_minToi)*(tmine/m_minToi);
473                 const float score = apen + tpen + cpen;
474
475                 // Update best score.
476                 if (score < bestScore)
477                 {
478                         bestDir = dir;
479                         bestToi = tmin;
480                         bestScore = score;
481                 }
482
483                 tc->dir[iter] = dir;
484                 tc->toi[iter] = tmin;
485                 tc->toie[iter] = tmine;
486         }
487
488         if (activeObst->m_vel.length() > 0.1)
489         {
490                 // Constrain max turn rate.
491                 float cura = atan2(activeObst->m_vel.y(),activeObst->m_vel.x());
492                 float da = bestDir - cura;
493                 if (da < -M_PI) da += (float)M_PI*2;
494                 if (da > M_PI) da -= (float)M_PI*2;
495                 if (da < -maxDeltaAngle)
496                 {
497                         bestDir = cura - maxDeltaAngle;
498                         bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n));
499                 }
500                 else if (da > maxDeltaAngle)
501                 {
502                         bestDir = cura + maxDeltaAngle;
503                         bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n));
504                 }
505         }
506
507         // Adjust speed when time of impact is less than min TOI.
508         if (bestToi < m_minToi)
509                 vmax *= bestToi/m_minToi;
510
511         // Constrain velocity change.
512         const float curSpeed = (float) activeObst->m_vel.length();
513         float deltaSpeed =  vmax - curSpeed; 
514         CLAMP(deltaSpeed, -maxDeltaSpeed, maxDeltaSpeed);
515         vmax = curSpeed + deltaSpeed;
516
517         // New steering velocity.
518         vel.x() = cosf(bestDir) * vmax;
519         vel.y() = sinf(bestDir) * vmax;
520
521         velocity.x() = vel.x();
522         velocity.y() = vel.y();
523 }