← Back to branch summary

~siretart/ubuntu/utopic/blender/libav10

« back to all changes in this revision

Viewing changes to source/gameengine/Ketsji/KX_ObstacleSimulation.cpp

  • Committer: Package Import Robot
  • Author(s): Matteo F. Vescovi
  • Date: 2012-07-23 08:54:18 UTC
  • mfrom: (14.2.16 sid)
  • mto: (14.2.19 sid)
  • mto: This revision was merged to the branch mainline in revision 42.
  • Revision ID: package-import@ubuntu.com-20120723085418-9foz30v6afaf5ffs
Tags: 2.63a-2
http://bugs.debian.org/658075
* debian/: Cycles support added (Closes: #658075)
  For now, this top feature has been enabled only
  on [any-amd64 any-i386] architectures because
  of OpenImageIO failing on all others
* debian/: scripts installation path changed
  from /usr/lib to /usr/share:
  + debian/patches/: patchset re-worked for path changing
  + debian/control: "Breaks" field added on yafaray-exporter

Show diffs side-by-side

added added

removed removed

Lines of Context:
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, 0.0, 1.0);
 
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
}