← Back to branch summary

~rebel/horde3d/trunk

« back to all changes in this revision

Viewing changes to trunk/Tools/Dependencies/RecastNavigation/DetourCrowd/Source/DetourObstacleAvoidance.cpp

  • Committer: felix
  • Date: 2015-07-07 12:57:07 UTC
  • Revision ID: svn-v4:5ce291ac-9df0-446f-9e4f-d57731c4dda7::1699
- Updated RecastNavigation to latest version and fixed multiple issues.
- Adapted GameDetourComponent, GameDetourCrowdComponent, DetourCrowdDemo and AAA accordingly.

Show diffs side-by-side

added added

removed removed

Lines of Context:
trunk/Tools/Dependencies/RecastNavigation/DetourCrowd/Source/DetourObstacleAvoidance.cpp
18
18
 
19
19
#include "DetourObstacleAvoidance.h"
20
20
#include "DetourCommon.h"
 
21
#include "DetourMath.h"
21
22
#include "DetourAlloc.h"
22
23
#include "DetourAssert.h"
23
24
#include <string.h>
24
 
#include <math.h>
25
25
#include <float.h>
26
26
#include <new>
27
27
 
44
44
        float d = b*b - a*c;
45
45
        if (d < 0.0f) return 0; // no intersection.
46
46
        a = 1.0f / a;
47
 
        const float rd = dtSqrt(d);
 
47
        const float rd = dtMathSqrtf(d);
48
48
        tmin = (b - rd) * a;
49
49
        tmax = (b + rd) * a;
50
50
        return 1;
58
58
        dtVsub(v,bq,bp);
59
59
        dtVsub(w,ap,bp);
60
60
        float d = dtVperp2D(u,v);
61
 
        if (fabsf(d) < 1e-6f) return 0;
 
61
        if (dtMathFabsf(d) < 1e-6f) return 0;
62
62
        d = 1.0f/d;
63
63
        t = dtVperp2D(v,w) * d;
64
64
        if (t < 0 || t > 1) return 0;
262
262
 
263
263
void dtObstacleAvoidanceQuery::addSegment(const float* p, const float* q)
264
264
{
265
 
        if (m_nsegments > m_maxSegments)
 
265
        if (m_nsegments >= m_maxSegments)
266
266
                return;
267
267
        
268
268
        dtObstacleSegment* seg = &m_segments[m_nsegments++];
281
281
                const float* pa = pos;
282
282
                const float* pb = cir->p;
283
283
                
284
 
                const float orig[3] = {0,0};
 
284
                const float orig[3] = {0,0,0};
285
285
                float dv[3];
286
286
                dtVsub(cir->dp,pb,pa);
287
287
                dtVnormalize(cir->dp);
311
311
        }       
312
312
}
313
313
 
 
314
 
 
315
/* Calculate the collision penalty for a given velocity vector
 
316
 * 
 
317
 * @param vcand sampled velocity
 
318
 * @param dvel desired velocity
 
319
 * @param minPenalty threshold penalty for early out
 
320
 */
314
321
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
315
322
                                                                                          const float* pos, const float rad,
316
323
                                                                                          const float* vel, const float* dvel,
 
324
                                                                                          const float minPenalty,
317
325
                                                                                          dtObstacleAvoidanceDebugData* debug)
318
326
{
 
327
        // penalty for straying away from the desired and current velocities
 
328
        const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
 
329
        const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
 
330
 
 
331
        // find the threshold hit time to bail out based on the early out penalty
 
332
        // (see how the penalty is calculated below to understnad)
 
333
        float minPen = minPenalty - vpen - vcpen;
 
334
        float tThresold = ((double)m_params.weightToi/(double)minPen - 0.1) * (double)m_params.horizTime;
 
335
        if (tThresold - m_params.horizTime > -FLT_EPSILON)
 
336
                return minPenalty; // already too much
 
337
 
319
338
        // Find min time of impact and exit amongst all obstacles.
320
339
        float tmin = m_params.horizTime;
321
340
        float side = 0;
350
369
                {
351
370
                        // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
352
371
                        if (htmin < tmin)
 
372
                        {
353
373
                                tmin = htmin;
 
374
                                if (tmin < tThresold)
 
375
                                        return minPenalty;
 
376
                        }
354
377
                }
355
378
        }
356
379
 
383
406
                
384
407
                // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
385
408
                if (htmin < tmin)
 
409
                {
386
410
                        tmin = htmin;
 
411
                        if (tmin < tThresold)
 
412
                                return minPenalty;
 
413
                }
387
414
        }
388
415
        
389
416
        // Normalize side bias, to prevent it dominating too much.
390
417
        if (nside)
391
418
                side /= nside;
392
419
        
393
 
        const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
394
 
        const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
395
420
        const float spen = m_params.weightSide * side;
396
421
        const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
397
422
        
414
439
        memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
415
440
        m_invHorizTime = 1.0f / m_params.horizTime;
416
441
        m_vmax = vmax;
417
 
        m_invVmax = 1.0f / vmax;
 
442
        m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
418
443
        
419
444
        dtVset(nvel, 0,0,0);
420
445
        
440
465
                        
441
466
                        if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
442
467
                        
443
 
                        const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, debug);
 
468
                        const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
444
469
                        ns++;
445
470
                        if (penalty < minPenalty)
446
471
                        {
454
479
}
455
480
 
456
481
 
 
482
// vector normalization that ignores the y-component.
 
483
inline void dtNormalize2D(float* v)
 
484
{
 
485
        float d = dtMathSqrtf(v[0] * v[0] + v[2] * v[2]);
 
486
        if (d==0)
 
487
                return;
 
488
        d = 1.0f / d;
 
489
        v[0] *= d;
 
490
        v[2] *= d;
 
491
}
 
492
 
 
493
// vector normalization that ignores the y-component.
 
494
inline void dtRorate2D(float* dest, const float* v, float ang)
 
495
{
 
496
        float c = cosf(ang);
 
497
        float s = sinf(ang);
 
498
        dest[0] = v[0]*c - v[2]*s;
 
499
        dest[2] = v[0]*s + v[2]*c;
 
500
        dest[1] = v[1];
 
501
}
 
502
 
 
503
 
457
504
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
458
505
                                                                                                         const float* vel, const float* dvel, float* nvel,
459
506
                                                                                                         const dtObstacleAvoidanceParams* params,
464
511
        memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
465
512
        m_invHorizTime = 1.0f / m_params.horizTime;
466
513
        m_vmax = vmax;
467
 
        m_invVmax = 1.0f / vmax;
 
514
        m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
468
515
        
469
516
        dtVset(nvel, 0,0,0);
470
517
        
481
528
        
482
529
        const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
483
530
        const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS);
 
531
        const int nd2 = nd / 2;
484
532
        const float da = (1.0f/nd) * DT_PI*2;
485
 
        const float dang = atan2f(dvel[2], dvel[0]);
486
 
        
 
533
        const float ca = cosf(da);
 
534
        const float sa = sinf(da);
 
535
 
 
536
        // desired direction
 
537
        float ddir[6];
 
538
        dtVcopy(ddir, dvel);
 
539
        dtNormalize2D(ddir);
 
540
        dtRorate2D (ddir+3, ddir, da*0.5f); // rotated by da/2
 
541
 
487
542
        // Always add sample at zero
488
543
        pat[npat*2+0] = 0;
489
544
        pat[npat*2+1] = 0;
492
547
        for (int j = 0; j < nr; ++j)
493
548
        {
494
549
                const float r = (float)(nr-j)/(float)nr;
495
 
                float a = dang + (j&1)*0.5f*da;
496
 
                for (int i = 0; i < nd; ++i)
497
 
                {
498
 
                        pat[npat*2+0] = cosf(a)*r;
499
 
                        pat[npat*2+1] = sinf(a)*r;
 
550
                pat[npat*2+0] = ddir[(j%1)*3] * r;
 
551
                pat[npat*2+1] = ddir[(j%1)*3+2] * r;
 
552
                float* last1 = pat + npat*2;
 
553
                float* last2 = last1;
 
554
                npat++;
 
555
 
 
556
                for (int i = 1; i < nd-1; i+=2)
 
557
                {
 
558
                        // get next point on the "right" (rotate CW)
 
559
                        pat[npat*2+0] = last1[0]*ca + last1[1]*sa;
 
560
                        pat[npat*2+1] = -last1[0]*sa + last1[1]*ca;
 
561
                        // get next point on the "left" (rotate CCW)
 
562
                        pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
 
563
                        pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
 
564
 
 
565
                        last1 = pat + npat*2;
 
566
                        last2 = last1 + 2;
 
567
                        npat += 2;
 
568
                }
 
569
 
 
570
                if ((nd&1) == 0)
 
571
                {
 
572
                        pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
 
573
                        pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
500
574
                        npat++;
501
 
                        a += da;
502
575
                }
503
576
        }
504
577
 
 
578
 
505
579
        // Start sampling.
506
580
        float cr = vmax * (1.0f - m_params.velBias);
507
581
        float res[3];
523
597
                        
524
598
                        if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
525
599
                        
526
 
                        const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug);
 
600
                        const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
527
601
                        ns++;
528
602
                        if (penalty < minPenalty)
529
603
                        {
541
615
        
542
616
        return ns;
543
617
}
544