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