DetourObstacleAvoidance.h

//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty.  In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
 
#ifndef DETOUROBSTACLEAVOIDANCE_H
#define DETOUROBSTACLEAVOIDANCE_H
 
struct dtObstacleCircle
{
    float p[3];             
    float vel[3];           
    float dvel[3];          
    float rad;              
    float dp[3], np[3];     
};
 
struct dtObstacleSegment
{
    float p[3], q[3];       
    bool touch;
};
 
 
class dtObstacleAvoidanceDebugData
{
public:
    dtObstacleAvoidanceDebugData();
    ~dtObstacleAvoidanceDebugData();
    
    bool init(const int maxSamples);
    void reset();
    void addSample(const float* vel, const float ssize, const float pen,
                   const float vpen, const float vcpen, const float spen, const float tpen);
    
    void normalizeSamples();
    
    inline int getSampleCount() const { return m_nsamples; }
    inline const float* getSampleVelocity(const int i) const { return &m_vel[i*3]; }
    inline float getSampleSize(const int i) const { return m_ssize[i]; }
    inline float getSamplePenalty(const int i) const { return m_pen[i]; }
    inline float getSampleDesiredVelocityPenalty(const int i) const { return m_vpen[i]; }
    inline float getSampleCurrentVelocityPenalty(const int i) const { return m_vcpen[i]; }
    inline float getSamplePreferredSidePenalty(const int i) const { return m_spen[i]; }
    inline float getSampleCollisionTimePenalty(const int i) const { return m_tpen[i]; }
 
private:
    // Explicitly disabled copy constructor and copy assignment operator.
    dtObstacleAvoidanceDebugData(const dtObstacleAvoidanceDebugData&);
    dtObstacleAvoidanceDebugData& operator=(const dtObstacleAvoidanceDebugData&);
 
    int m_nsamples;
    int m_maxSamples;
    float* m_vel;
    float* m_ssize;
    float* m_pen;
    float* m_vpen;
    float* m_vcpen;
    float* m_spen;
    float* m_tpen;
};
 
dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData();
void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr);
 
 
static const int DT_MAX_PATTERN_DIVS = 32;  
static const int DT_MAX_PATTERN_RINGS = 4;  
 
struct dtObstacleAvoidanceParams
{
    float velBias;
    float weightDesVel;
    float weightCurVel;
    float weightSide;
    float weightToi;
    float horizTime;
    unsigned char gridSize; 
    unsigned char adaptiveDivs; 
    unsigned char adaptiveRings;    
    unsigned char adaptiveDepth;    
};
 
class dtObstacleAvoidanceQuery
{
public:
    dtObstacleAvoidanceQuery();
    ~dtObstacleAvoidanceQuery();
    
    bool init(const int maxCircles, const int maxSegments);
    
    void reset();
 
    void addCircle(const float* pos, const float rad,
                   const float* vel, const float* dvel);
                   
    void addSegment(const float* p, const float* q);
 
    int sampleVelocityGrid(const float* pos, const float rad, const float vmax,
                           const float* vel, const float* dvel, float* nvel,
                           const dtObstacleAvoidanceParams* params,
                           dtObstacleAvoidanceDebugData* debug = 0);
 
    int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
                               const float* vel, const float* dvel, float* nvel,
                               const dtObstacleAvoidanceParams* params, 
                               dtObstacleAvoidanceDebugData* debug = 0);
    
    inline int getObstacleCircleCount() const { return m_ncircles; }
    const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; }
 
    inline int getObstacleSegmentCount() const { return m_nsegments; }
    const dtObstacleSegment* getObstacleSegment(const int i) { return &m_segments[i]; }
 
private:
    // Explicitly disabled copy constructor and copy assignment operator.
    dtObstacleAvoidanceQuery(const dtObstacleAvoidanceQuery&);
    dtObstacleAvoidanceQuery& operator=(const dtObstacleAvoidanceQuery&);
 
    void prepare(const float* pos, const float* dvel);
 
    float processSample(const float* vcand, const float cs,
                        const float* pos, const float rad,
                        const float* vel, const float* dvel,
                        const float minPenalty,
                        dtObstacleAvoidanceDebugData* debug);
 
    dtObstacleAvoidanceParams m_params;
    float m_invHorizTime;
    float m_vmax;
    float m_invVmax;
 
    int m_maxCircles;
    dtObstacleCircle* m_circles;
    int m_ncircles;
 
    int m_maxSegments;
    dtObstacleSegment* m_segments;
    int m_nsegments;
};
 
dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery();
void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr);
 
 
#endif // DETOUROBSTACLEAVOIDANCE_H