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