bfbdb4a1f6af2dd2a104ba8cba2ef24726e548c4
[blender.git] / source / gameengine / Ketsji / KX_ObstacleSimulation.cpp
1 /**
2 * Simulation for obstacle avoidance behavior
3 *
4 *
5 * ***** BEGIN GPL LICENSE BLOCK *****
6 *
7 * This program is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU General Public License
9 * as published by the Free Software Foundation; either version 2
10 * of the License, or (at your option) any later version. The Blender
11 * Foundation also sells licenses for use in proprietary software under
12 * the Blender License.  See http://www.blender.org/BL/ for information
13 * about this.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18 * GNU General Public License for more details.
19 *
20 * You should have received a copy of the GNU General Public License
21 * along with this program; if not, write to the Free Software Foundation,
22 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
23 *
24 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
25 * All rights reserved.
26 *
27 * The Original Code is: all of this file.
28 *
29 * Contributor(s): none yet.
30 *
31 * ***** END GPL LICENSE BLOCK *****
32 */
33
34 #include "KX_ObstacleSimulation.h"
35 #include "KX_NavMeshObject.h"
36 #include "KX_PythonInit.h"
37 #include "DNA_object_types.h"
38 #include "BLI_math.h"
39
40 namespace
41 {
42         inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); }
43
44         inline float sqr(float x) { return x*x; }
45         inline float lerp(float a, float b, float t) { return a + (b-a)*t; }
46         inline float clamp(float a, float mn, float mx) { return a < mn ? mn : (a > mx ? mx : a); }
47
48         inline float vdistsqr(const float* a, const float* b) { return sqr(b[0]-a[0]) + sqr(b[1]-a[1]); }
49         inline float vdist(const float* a, const float* b) { return sqrtf(vdistsqr(a,b)); }
50         inline void vcpy(float* a, const float* b) { a[0]=b[0]; a[1]=b[1]; }
51         inline float vdot(const float* a, const float* b) { return a[0]*b[0] + a[1]*b[1]; }
52         inline float vperp(const float* a, const float* b) { return a[0]*b[1] - a[1]*b[0]; }
53         inline void vsub(float* v, const float* a, const float* b) { v[0] = a[0]-b[0]; v[1] = a[1]-b[1]; }
54         inline void vadd(float* v, const float* a, const float* b) { v[0] = a[0]+b[0]; v[1] = a[1]+b[1]; }
55         inline void vscale(float* v, const float* a, const float s) { v[0] = a[0]*s; v[1] = a[1]*s; }
56         inline void vset(float* v, float x, float y) { v[0]=x; v[1]=y; }
57         inline float vlensqr(const float* v) { return vdot(v,v); }
58         inline float vlen(const float* v) { return sqrtf(vlensqr(v)); }
59         inline void vlerp(float* v, const float* a, const float* b, float t) { v[0] = lerp(a[0], b[0], t); v[1] = lerp(a[1], b[1], t); }
60         inline void vmad(float* v, const float* a, const float* b, float s) { v[0] = a[0] + b[0]*s; v[1] = a[1] + b[1]*s; }
61         inline void vnorm(float* v)
62         {
63                 float d = vlen(v);
64                 if (d > 0.0001f)
65                 {
66                         d = 1.0f/d;
67                         v[0] *= d;
68                         v[1] *= d;
69                 }
70         }
71 }
72 inline float triarea(const float* a, const float* b, const float* c)
73 {
74         return (b[0]*a[1] - a[0]*b[1]) + (c[0]*b[1] - b[0]*c[1]) + (a[0]*c[1] - c[0]*a[1]);
75 }
76
77 static void closestPtPtSeg(const float* pt,
78                                         const float* sp, const float* sq,
79                                         float& t)
80 {
81         float dir[2],diff[3];
82         vsub(dir,sq,sp);
83         vsub(diff,pt,sp);
84         t = vdot(diff,dir);
85         if (t <= 0.0f) { t = 0; return; }
86         float d = vdot(dir,dir);
87         if (t >= d) { t = 1; return; }
88         t /= d;
89 }
90
91 static float distPtSegSqr(const float* pt, const float* sp, const float* sq)
92 {
93         float t;
94         closestPtPtSeg(pt, sp,sq, t);
95         float np[2];
96         vlerp(np, sp,sq, t);
97         return vdistsqr(pt,np);
98 }
99
100 static int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
101                                           const MT_Vector3& pos1, const MT_Scalar r1,
102                                           float& tmin, float& tmax)
103 {
104         static const float EPS = 0.0001f;
105         MT_Vector2 c0(pos0.x(), pos0.y());
106         MT_Vector2 c1(pos1.x(), pos1.y());
107         MT_Vector2 s = c1 - c0;
108         MT_Scalar  r = r0+r1;
109         float c = s.length2() - r*r;
110         float a = v.length2();
111         if (a < EPS) return 0;  // not moving
112
113         // Overlap, calc time to exit.
114         float b = MT_dot(v,s);
115         float d = b*b - a*c;
116         if (d < 0.0f) return 0; // no intersection.
117         tmin = (b - sqrtf(d)) / a;
118         tmax = (b + sqrtf(d)) / a;
119         return 1;
120 }
121
122 static int sweepCircleSegment(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
123                                            const MT_Vector3& pa, const MT_Vector3& pb, const MT_Scalar sr,
124                                            float& tmin, float &tmax)
125 {
126         // equation parameters
127         MT_Vector2 c0(pos0.x(), pos0.y());
128         MT_Vector2 sa(pa.x(), pa.y());
129         MT_Vector2 sb(pb.x(), pb.y());
130         MT_Vector2 L = sb-sa;
131         MT_Vector2 H = c0-sa;
132         MT_Scalar radius = r0+sr;
133         float l2 = L.length2();
134         float r2 = radius * radius;
135         float dl = perp(v, L);
136         float hl = perp(H, L);
137         float a = dl * dl;
138         float b = 2.0f * hl * dl;
139         float c = hl * hl - (r2 * l2);
140         float d = (b*b) - (4.0f * a * c);
141
142         // infinite line missed by infinite ray.
143         if (d < 0.0f)
144                 return 0;
145
146         d = sqrtf(d);
147         tmin = (-b - d) / (2.0f * a);
148         tmax = (-b + d) / (2.0f * a);
149
150         // line missed by ray range.
151         /*      if (tmax < 0.0f || tmin > 1.0f)
152         return 0;*/
153
154         // find what part of the ray was collided.
155         MT_Vector2 Pedge;
156         Pedge = c0+v*tmin;
157         H = Pedge - sa;
158         float e0 = MT_dot(H, L) / l2;
159         Pedge = c0 + v*tmax;
160         H = Pedge - sa;
161         float e1 = MT_dot(H, L) / l2;
162
163         if (e0 < 0.0f || e1 < 0.0f)
164         {
165                 float ctmin, ctmax;
166                 if (sweepCircleCircle(pos0, r0, v, pa, sr, ctmin, ctmax))
167                 {
168                         if (e0 < 0.0f && ctmin > tmin)
169                                 tmin = ctmin;
170                         if (e1 < 0.0f && ctmax < tmax)
171                                 tmax = ctmax;
172                 }
173                 else
174                 {
175                         return 0;
176                 }
177         }
178
179         if (e0 > 1.0f || e1 > 1.0f)
180         {
181                 float ctmin, ctmax;
182                 if (sweepCircleCircle(pos0, r0, v, pb, sr, ctmin, ctmax))
183                 {
184                         if (e0 > 1.0f && ctmin > tmin)
185                                 tmin = ctmin;
186                         if (e1 > 1.0f && ctmax < tmax)
187                                 tmax = ctmax;
188                 }
189                 else
190                 {
191                         return 0;
192                 }
193         }
194
195         return 1;
196 }
197
198 static bool inBetweenAngle(float a, float amin, float amax, float& t)
199 {
200         if (amax < amin) amax += (float)M_PI*2;
201         if (a < amin-(float)M_PI) a += (float)M_PI*2;
202         if (a > amin+(float)M_PI) a -= (float)M_PI*2;
203         if (a >= amin && a < amax)
204         {
205                 t = (a-amin) / (amax-amin);
206                 return true;
207         }
208         return false;
209 }
210
211 static float interpolateToi(float a, const float* dir, const float* toi, const int ntoi)
212 {
213         for (int i = 0; i < ntoi; ++i)
214         {
215                 int next = (i+1) % ntoi;
216                 float t;
217                 if (inBetweenAngle(a, dir[i], dir[next], t))
218                 {
219                         return lerp(toi[i], toi[next], t);
220                 }
221         }
222         return 0;
223 }
224
225 KX_ObstacleSimulation::KX_ObstacleSimulation(MT_Scalar levelHeight, bool enableVisualization)
226 :       m_levelHeight(levelHeight)
227 ,       m_enableVisualization(enableVisualization)
228 {
229
230 }
231
232 KX_ObstacleSimulation::~KX_ObstacleSimulation()
233 {
234         for (size_t i=0; i<m_obstacles.size(); i++)
235         {
236                 KX_Obstacle* obs = m_obstacles[i];
237                 delete obs;
238         }
239         m_obstacles.clear();
240 }
241 KX_Obstacle* KX_ObstacleSimulation::CreateObstacle(KX_GameObject* gameobj)
242 {
243         KX_Obstacle* obstacle = new KX_Obstacle();
244         obstacle->m_gameObj = gameobj;
245
246         vset(obstacle->vel, 0,0);
247         vset(obstacle->pvel, 0,0);
248         vset(obstacle->dvel, 0,0);
249         vset(obstacle->nvel, 0,0);
250         for (int i = 0; i < VEL_HIST_SIZE; ++i)
251                 vset(&obstacle->hvel[i*2], 0,0);
252         obstacle->hhead = 0;
253
254         gameobj->RegisterObstacle(this);
255         m_obstacles.push_back(obstacle);
256         return obstacle;
257 }
258
259 void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj)
260 {
261         KX_Obstacle* obstacle = CreateObstacle(gameobj);
262         struct Object* blenderobject = gameobj->GetBlenderObject();
263         obstacle->m_type = KX_OBSTACLE_OBJ;
264         obstacle->m_shape = KX_OBSTACLE_CIRCLE;
265         obstacle->m_rad = blenderobject->obstacleRad;
266 }
267
268 void KX_ObstacleSimulation::AddObstaclesForNavMesh(KX_NavMeshObject* navmeshobj)
269 {       
270         dtStatNavMesh* navmesh = navmeshobj->GetNavMesh();
271         if (navmesh)
272         {
273                 int npoly = navmesh->getPolyCount();
274                 for (int pi=0; pi<npoly; pi++)
275                 {
276                         const dtStatPoly* poly = navmesh->getPoly(pi);
277
278                         for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
279                         {       
280                                 if (poly->n[j]) continue;
281                                 const float* vj = navmesh->getVertex(poly->v[j]);
282                                 const float* vi = navmesh->getVertex(poly->v[i]);
283                 
284                                 KX_Obstacle* obstacle = CreateObstacle(navmeshobj);
285                                 obstacle->m_type = KX_OBSTACLE_NAV_MESH;
286                                 obstacle->m_shape = KX_OBSTACLE_SEGMENT;
287                                 obstacle->m_pos = MT_Point3(vj[0], vj[2], vj[1]);
288                                 obstacle->m_pos2 = MT_Point3(vi[0], vi[2], vi[1]);
289                                 obstacle->m_rad = 0;
290                         }
291                 }
292         }
293 }
294
295 void KX_ObstacleSimulation::DestroyObstacleForObj(KX_GameObject* gameobj)
296 {
297         for (size_t i=0; i<m_obstacles.size(); )
298         {
299                 if (m_obstacles[i]->m_gameObj == gameobj)
300                 {
301                         KX_Obstacle* obstacle = m_obstacles[i];
302                         obstacle->m_gameObj->UnregisterObstacle();
303                         m_obstacles[i] = m_obstacles.back();
304                         m_obstacles.pop_back();
305                         delete obstacle;
306                 }
307                 else
308                         i++;
309         }
310 }
311
312 void KX_ObstacleSimulation::UpdateObstacles()
313 {
314         for (size_t i=0; i<m_obstacles.size(); i++)
315         {
316                 if (m_obstacles[i]->m_type==KX_OBSTACLE_NAV_MESH || m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
317                         continue;
318
319                 KX_Obstacle* obs = m_obstacles[i];
320                 obs->m_pos = obs->m_gameObj->NodeGetWorldPosition();
321                 obs->vel[0] = obs->m_gameObj->GetLinearVelocity().x();
322                 obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y();
323
324                 // Update velocity history and calculate perceived (average) velocity.
325                 vcpy(&obs->hvel[obs->hhead*2], obs->vel);
326                 obs->hhead = (obs->hhead+1) % VEL_HIST_SIZE;
327                 vset(obs->pvel,0,0);
328                 for (int j = 0; j < VEL_HIST_SIZE; ++j)
329                         vadd(obs->pvel, obs->pvel, &obs->hvel[j*2]);
330                 vscale(obs->pvel, obs->pvel, 1.0f/VEL_HIST_SIZE);
331         }
332 }
333
334 KX_Obstacle* KX_ObstacleSimulation::GetObstacle(KX_GameObject* gameobj)
335 {
336         for (size_t i=0; i<m_obstacles.size(); i++)
337         {
338                 if (m_obstacles[i]->m_gameObj == gameobj)
339                         return m_obstacles[i];
340         }
341
342         return NULL;
343 }
344
345 void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
346                                                                                 MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle)
347 {
348 }
349
350 void KX_ObstacleSimulation::DrawObstacles()
351 {
352         if (!m_enableVisualization)
353                 return;
354         static const MT_Vector3 bluecolor(0,0,1);
355         static const MT_Vector3 normal(0.,0.,1.);
356         static const int SECTORS_NUM = 32;
357         for (size_t i=0; i<m_obstacles.size(); i++)
358         {
359                 if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
360                 {
361                         MT_Point3 p1 = m_obstacles[i]->m_pos;
362                         MT_Point3 p2 = m_obstacles[i]->m_pos2;
363                         //apply world transform
364                         if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH)
365                         {
366                                 KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(m_obstacles[i]->m_gameObj);
367                                 p1 = navmeshobj->TransformToWorldCoords(p1);
368                                 p2 = navmeshobj->TransformToWorldCoords(p2);
369                         }
370
371                         KX_RasterizerDrawDebugLine(p1, p2, bluecolor);
372                 }
373                 else if (m_obstacles[i]->m_shape==KX_OBSTACLE_CIRCLE)
374                 {
375                         KX_RasterizerDrawDebugCircle(m_obstacles[i]->m_pos, m_obstacles[i]->m_rad, bluecolor,
376                                                                                 normal, SECTORS_NUM);
377                 }
378         }       
379 }
380
381 static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle)
382 {
383         switch (obstacle->m_shape)
384         {
385         case KX_OBSTACLE_SEGMENT :
386         {
387                 MT_Vector3 ab = obstacle->m_pos2 - obstacle->m_pos;
388                 if (!ab.fuzzyZero())
389                 {
390                         MT_Vector3 abdir = ab.normalized();
391                         MT_Vector3  v = pos - obstacle->m_pos;
392                         MT_Scalar proj = abdir.dot(v);
393                         CLAMP(proj, 0, ab.length());
394                         MT_Point3 res = obstacle->m_pos + abdir*proj;
395                         return res;
396                 }               
397         }
398         case KX_OBSTACLE_CIRCLE :
399         default:
400                 return obstacle->m_pos;
401         }
402 }
403
404 static bool filterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst,
405                                                         float levelHeight)
406 {
407         //filter obstacles by type
408         if ( (otherObst == activeObst) ||
409                 (otherObst->m_type==KX_OBSTACLE_NAV_MESH && otherObst->m_gameObj!=activeNavMeshObj)     )
410                 return false;
411
412         //filter obstacles by position
413         MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst);
414         if ( fabs(activeObst->m_pos.z() - p.z()) > levelHeight)
415                 return false;
416
417         return true;
418 }
419
420 ///////////*********TOI_rays**********/////////////////
421 KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI(MT_Scalar levelHeight, bool enableVisualization)
422 :       KX_ObstacleSimulation(levelHeight, enableVisualization),
423         m_maxSamples(32),
424         m_minToi(0.0f),
425         m_maxToi(0.0f),
426         m_velWeight(1.0f),
427         m_curVelWeight(1.0f),
428         m_toiWeight(1.0f),
429         m_collisionWeight(1.0f)
430 {
431 }
432
433
434 void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
435                                                                                                                    MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle)
436 {
437         int nobs = m_obstacles.size();
438         int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
439         if (obstidx == nobs)
440                 return;
441
442         vset(activeObst->dvel, velocity.x(), velocity.y());
443
444         //apply RVO
445         sampleRVO(activeObst, activeNavMeshObj, maxDeltaAngle);
446
447         // Fake dynamic constraint.
448         float dv[2];
449         float vel[2];
450         vsub(dv, activeObst->nvel, activeObst->vel);
451         float ds = vlen(dv);
452         if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
453                 vscale(dv, dv, fabs(maxDeltaSpeed/ds));
454         vadd(vel, activeObst->vel, dv);
455
456         velocity.x() = vel[0];
457         velocity.y() = vel[1];  
458 }
459
460 ///////////*********TOI_rays**********/////////////////
461 static const int AVOID_MAX_STEPS = 128;
462 struct TOICircle
463 {
464         TOICircle() : n(0), minToi(0), maxToi(1) {}
465         float   toi[AVOID_MAX_STEPS];   // Time of impact (seconds)
466         float   toie[AVOID_MAX_STEPS];  // Time of exit (seconds)
467         float   dir[AVOID_MAX_STEPS];   // Direction (radians)
468         int             n;                                              // Number of samples
469         float   minToi, maxToi;                 // Min/max TOI (seconds)
470 };
471
472 KX_ObstacleSimulationTOI_rays::KX_ObstacleSimulationTOI_rays(MT_Scalar levelHeight, bool enableVisualization):
473         KX_ObstacleSimulationTOI(levelHeight, enableVisualization)
474 {
475         m_maxSamples = 32;
476         m_minToi = 0.5f;
477         m_maxToi = 1.2f;
478         m_velWeight = 4.0f;
479         m_toiWeight = 1.0f;
480         m_collisionWeight = 100.0f;
481 }
482
483
484 void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
485                                                                                 const float maxDeltaAngle)
486 {
487         MT_Vector2 vel(activeObst->dvel[0], activeObst->dvel[1]);
488         float vmax = (float) vel.length();
489         float odir = (float) atan2(vel.y(), vel.x());
490
491         MT_Vector2 ddir = vel;
492         ddir.normalize();
493
494         float bestScore = FLT_MAX;
495         float bestDir = odir;
496         float bestToi = 0;
497
498         TOICircle tc;
499         tc.n = m_maxSamples;
500         tc.minToi = m_minToi;
501         tc.maxToi = m_maxToi;
502
503         const int iforw = m_maxSamples/2;
504         const float aoff = (float)iforw / (float)m_maxSamples;
505
506         size_t nobs = m_obstacles.size();
507         for (int iter = 0; iter < m_maxSamples; ++iter)
508         {
509                 // Calculate sample velocity
510                 const float ndir = ((float)iter/(float)m_maxSamples) - aoff;
511                 const float dir = odir+ndir*M_PI*2;
512                 MT_Vector2 svel;
513                 svel.x() = cosf(dir) * vmax;
514                 svel.y() = sinf(dir) * vmax;
515
516                 // Find min time of impact and exit amongst all obstacles.
517                 float tmin = m_maxToi;
518                 float tmine = 0;
519                 for (int i = 0; i < nobs; ++i)
520                 {
521                         KX_Obstacle* ob = m_obstacles[i];
522                         bool res = filterObstacle(activeObst, activeNavMeshObj, ob, m_levelHeight);
523                         if (!res)
524                                 continue;
525
526                         float htmin,htmax;
527
528                         if (ob->m_shape == KX_OBSTACLE_CIRCLE)
529                         {
530                                 MT_Vector2 vab;
531                                 if (vlen(ob->vel) < 0.01f*0.01f)
532                                 {
533                                         // Stationary, use VO
534                                         vab = svel;
535                                 }
536                                 else
537                                 {
538                                         // Moving, use RVO
539                                         vab = 2*svel - vel - ob->vel;
540                                 }
541
542                                 if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, 
543                                         vab, ob->m_pos, ob->m_rad, htmin, htmax))
544                                         continue;
545                         }
546                         else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
547                         {
548                                 MT_Point3 p1 = ob->m_pos;
549                                 MT_Point3 p2 = ob->m_pos2;
550                                 //apply world transform
551                                 if (ob->m_type == KX_OBSTACLE_NAV_MESH)
552                                 {
553                                         KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
554                                         p1 = navmeshobj->TransformToWorldCoords(p1);
555                                         p2 = navmeshobj->TransformToWorldCoords(p2);
556                                 }
557                                 if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel, 
558                                         p1, p2, ob->m_rad, htmin, htmax))
559                                         continue;
560                         }
561
562                         if (htmin > 0.0f)
563                         {
564                                 // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
565                                 if (htmin < tmin)
566                                         tmin = htmin;
567                         }
568                         else if (htmax > 0.0f)
569                         {
570                                 // The agent overlaps the obstacle, keep track of first safe exit.
571                                 if (htmax > tmine)
572                                         tmine = htmax;
573                         }
574                 }
575
576                 // Calculate sample penalties and final score.
577                 const float apen = m_velWeight * fabsf(ndir);
578                 const float tpen = m_toiWeight * (1.0f/(0.0001f+tmin/m_maxToi));
579                 const float cpen = m_collisionWeight * (tmine/m_minToi)*(tmine/m_minToi);
580                 const float score = apen + tpen + cpen;
581
582                 // Update best score.
583                 if (score < bestScore)
584                 {
585                         bestDir = dir;
586                         bestToi = tmin;
587                         bestScore = score;
588                 }
589
590                 tc.dir[iter] = dir;
591                 tc.toi[iter] = tmin;
592                 tc.toie[iter] = tmine;
593         }
594
595         if (vlen(activeObst->vel) > 0.1)
596         {
597                 // Constrain max turn rate.
598                 float cura = atan2(activeObst->vel[1],activeObst->vel[0]);
599                 float da = bestDir - cura;
600                 if (da < -M_PI) da += (float)M_PI*2;
601                 if (da > M_PI) da -= (float)M_PI*2;
602                 if (da < -maxDeltaAngle)
603                 {
604                         bestDir = cura - maxDeltaAngle;
605                         bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
606                 }
607                 else if (da > maxDeltaAngle)
608                 {
609                         bestDir = cura + maxDeltaAngle;
610                         bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
611                 }
612         }
613
614         // Adjust speed when time of impact is less than min TOI.
615         if (bestToi < m_minToi)
616                 vmax *= bestToi/m_minToi;
617
618         // New steering velocity.
619         activeObst->nvel[0] = cosf(bestDir) * vmax;
620         activeObst->nvel[1] = sinf(bestDir) * vmax;
621 }
622
623 ///////////********* TOI_cells**********/////////////////
624
625 static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
626                                                    KX_Obstacles& obstacles,  float levelHeight, const float vmax,
627                                                    const float* spos, const float cs, const int nspos, float* res,                                                 
628                                                    float maxToi, float velWeight, float curVelWeight, float sideWeight,
629                                                    float toiWeight)
630 {
631         vset(res, 0,0);
632
633         const float ivmax = 1.0f / vmax;
634
635         float adir[2] /*, adist */;
636         vcpy(adir, activeObst->pvel);
637         if (vlen(adir) > 0.01f)
638                 vnorm(adir);
639         else
640                 vset(adir,0,0);
641         float activeObstPos[2];
642         vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y()); 
643         /* adist = vdot(adir, activeObstPos); */
644
645         float minPenalty = FLT_MAX;
646
647         for (int n = 0; n < nspos; ++n)
648         {
649                 float vcand[2];
650                 vcpy(vcand, &spos[n*2]);                
651
652                 // Find min time of impact and exit amongst all obstacles.
653                 float tmin = maxToi;
654                 float side = 0;
655                 int nside = 0;
656
657                 for (int i = 0; i < obstacles.size(); ++i)
658                 {
659                         KX_Obstacle* ob = obstacles[i];
660                         bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
661                         if (!res)
662                                 continue;
663                         float htmin, htmax;
664
665                         if (ob->m_shape==KX_OBSTACLE_CIRCLE)
666                         {
667                                 float vab[2];
668
669                                 // Moving, use RVO
670                                 vscale(vab, vcand, 2);
671                                 vsub(vab, vab, activeObst->vel);
672                                 vsub(vab, vab, ob->vel);
673
674                                 // Side
675                                 // NOTE: dp, and dv are constant over the whole calculation,
676                                 // they can be precomputed per object. 
677                                 const float* pa = activeObstPos;
678                                 float pb[2];
679                                 vset(pb, ob->m_pos.x(), ob->m_pos.y());
680
681                                 const float orig[2] = {0,0};
682                                 float dp[2],dv[2],np[2];
683                                 vsub(dp,pb,pa);
684                                 vnorm(dp);
685                                 vsub(dv,ob->dvel, activeObst->dvel);
686
687                                 const float a = triarea(orig, dp,dv);
688                                 if (a < 0.01f)
689                                 {
690                                         np[0] = -dp[1];
691                                         np[1] = dp[0];
692                                 }
693                                 else
694                                 {
695                                         np[0] = dp[1];
696                                         np[1] = -dp[0];
697                                 }
698
699                                 side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f);
700                                 nside++;
701
702                                 if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad, 
703                                         htmin, htmax))
704                                         continue;
705
706                                 // Handle overlapping obstacles.
707                                 if (htmin < 0.0f && htmax > 0.0f)
708                                 {
709                                         // Avoid more when overlapped.
710                                         htmin = -htmin * 0.5f;
711                                 }
712                         }
713                         else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
714                         {
715                                 MT_Point3 p1 = ob->m_pos;
716                                 MT_Point3 p2 = ob->m_pos2;
717                                 //apply world transform
718                                 if (ob->m_type == KX_OBSTACLE_NAV_MESH)
719                                 {
720                                         KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
721                                         p1 = navmeshobj->TransformToWorldCoords(p1);
722                                         p2 = navmeshobj->TransformToWorldCoords(p2);
723                                 }
724                                 float p[2], q[2];
725                                 vset(p, p1.x(), p1.y());
726                                 vset(q, p2.x(), p2.y());
727
728                                 // NOTE: the segments are assumed to come from a navmesh which is shrunken by
729                                 // the agent radius, hence the use of really small radius.
730                                 // This can be handle more efficiently by using seg-seg test instead.
731                                 // If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
732                                 const float r = 0.01f; // agent->rad
733                                 if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad))
734                                 {
735                                         float sdir[2], snorm[2];
736                                         vsub(sdir, q, p);
737                                         snorm[0] = sdir[1];
738                                         snorm[1] = -sdir[0];
739                                         // If the velocity is pointing towards the segment, no collision.
740                                         if (vdot(snorm, vcand) < 0.0f)
741                                                 continue;
742                                         // Else immediate collision.
743                                         htmin = 0.0f;
744                                         htmax = 10.0f;
745                                 }
746                                 else
747                                 {
748                                         if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
749                                                 continue;
750                                 }
751
752                                 // Avoid less when facing walls.
753                                 htmin *= 2.0f;
754                         }
755
756                         if (htmin >= 0.0f)
757                         {
758                                 // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
759                                 if (htmin < tmin)
760                                         tmin = htmin;
761                         }
762                 }
763
764                 // Normalize side bias, to prevent it dominating too much.
765                 if (nside)
766                         side /= nside;
767
768                 const float vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
769                 const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
770                 const float spen = sideWeight * side;
771                 const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));
772
773                 const float penalty = vpen + vcpen + spen + tpen;
774
775                 if (penalty < minPenalty)
776                 {
777                         minPenalty = penalty;
778                         vcpy(res, vcand);
779                 }
780         }
781 }
782
783 void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
784                                            const float maxDeltaAngle)
785 {
786         vset(activeObst->nvel, 0.f, 0.f);
787         float vmax = vlen(activeObst->dvel);
788
789         float* spos = new float[2*m_maxSamples];
790         int nspos = 0;
791
792         if (!m_adaptive)
793         {
794                 const float cvx = activeObst->dvel[0]*m_bias;
795                 const float cvy = activeObst->dvel[1]*m_bias;
796                 float vmax = vlen(activeObst->dvel);
797                 const float vrange = vmax*(1-m_bias);
798                 const float cs = 1.0f / (float)m_sampleRadius*vrange;
799
800                 for (int y = -m_sampleRadius; y <= m_sampleRadius; ++y)
801                 {
802                         for (int x = -m_sampleRadius; x <= m_sampleRadius; ++x)
803                         {
804                                 if (nspos < m_maxSamples)
805                                 {
806                                         const float vx = cvx + (float)(x+0.5f)*cs;
807                                         const float vy = cvy + (float)(y+0.5f)*cs;
808                                         if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
809                                         spos[nspos*2+0] = vx;
810                                         spos[nspos*2+1] = vy;
811                                         nspos++;
812                                 }
813                         }
814                 }
815                 processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2, 
816                         nspos,  activeObst->nvel, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight);
817         }
818         else
819         {
820                 int rad;
821                 float res[2];
822                 float cs;
823                 // First sample location.
824                 rad = 4;
825                 res[0] = activeObst->dvel[0]*m_bias;
826                 res[1] = activeObst->dvel[1]*m_bias;
827                 cs = vmax*(2-m_bias*2) / (float)(rad-1);
828
829                 for (int k = 0; k < 5; ++k)
830                 {
831                         const float half = (rad-1)*cs*0.5f;
832
833                         nspos = 0;
834                         for (int y = 0; y < rad; ++y)
835                         {
836                                 for (int x = 0; x < rad; ++x)
837                                 {
838                                         const float vx = res[0] + x*cs - half;
839                                         const float vy = res[1] + y*cs - half;
840                                         if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
841                                         spos[nspos*2+0] = vx;
842                                         spos[nspos*2+1] = vy;
843                                         nspos++;
844                                 }
845                         }
846
847                         processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2, 
848                                 nspos,  res, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight);
849
850                         cs *= 0.5f;
851                 }
852                 vcpy(activeObst->nvel, res);
853         }
854 }
855
856 KX_ObstacleSimulationTOI_cells::KX_ObstacleSimulationTOI_cells(MT_Scalar levelHeight, bool enableVisualization)
857 :       KX_ObstacleSimulationTOI(levelHeight, enableVisualization)
858 ,       m_bias(0.4f)
859 ,       m_adaptive(true)
860 ,       m_sampleRadius(15)
861 {
862         m_maxSamples = (m_sampleRadius*2+1)*(m_sampleRadius*2+1) + 100;
863         m_maxToi = 1.5f;
864         m_velWeight = 2.0f;
865         m_curVelWeight = 0.75f;
866         m_toiWeight = 2.5f;
867         m_collisionWeight = 0.75f; //side_weight
868 }