Revision: 31050 http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=31050 Author: nicks Date: 2010-08-04 21:32:37 +0200 (Wed, 04 Aug 2010)
Log Message: ----------- integrated adaptive sampling algorithm for obstacle avoidance Modified Paths: -------------- branches/soc-2010-nicks/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.h branches/soc-2010-nicks/source/gameengine/Ketsji/KX_SteeringActuator.cpp Removed Paths: ------------- branches/soc-2010-nicks/extern/recastnavigation/BlenderNavMesh/ Modified: branches/soc-2010-nicks/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj =================================================================== --- branches/soc-2010-nicks/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj 2010-08-04 17:23:13 UTC (rev 31049) +++ branches/soc-2010-nicks/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj 2010-08-04 19:32:37 UTC (rev 31050) @@ -271,7 +271,7 @@ Name="VCCLCompilerTool" Optimization="2" InlineFunctionExpansion="1" - AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\build\msvc_9\extern\recastnavigation\Detour\Include;..\..\..\..\build\msvc_9\extern\recastnavigation\Recast\Include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\sdl\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\python\generic;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenloader;..\..\..\source\blender\blenkernel;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\networ k;..\..\..\source\gameengine\Converter;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\scenegraph;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\physics\sumo\include;..\..\..\source\gameengine\physics\common\dummy;..\..\..\source\gameengine\Rasterizer\RAS_OpenGLRasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\sumo\include;..\..\..\source\sumo\fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\python\api2_2x;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern" + AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\build\msvc_9\extern\recastnavigation\Detour\Include;..\..\..\..\build\msvc_9\extern\recastnavigation\Recast\Include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\sdl\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\python\generic;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenloader;..\..\..\source\blender\blenkernel;..\..\..\source\blender\editors\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rast erizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\Converter;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\scenegraph;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\physics\sumo\include;..\..\..\source\gameengine\physics\common\dummy;..\..\..\source\gameengine\Rasterizer\RAS_OpenGLRasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\sumo\include;..\..\..\source\sumo\fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\python\api2_2x;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern" PreprocessorDefinitions="NDEBUG;WIN32;_LIB;USE_SUMO_SOLID;WITH_GLEXT;GLEW_STATIC;WITH_FFMPEG" StringPooling="true" RuntimeLibrary="0" Modified: branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp =================================================================== --- branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp 2010-08-04 17:23:13 UTC (rev 31049) +++ branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp 2010-08-04 19:32:37 UTC (rev 31050) @@ -38,9 +38,66 @@ #include "DNA_object_types.h" #include "BLI_math.h" -inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); } -inline float lerp(float a, float b, float t) { return a + (b-a)*t; } +namespace +{ + inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); } + inline float sqr(float x) { return x*x; } + inline float lerp(float a, float b, float t) { return a + (b-a)*t; } + inline float clamp(float a, float mn, float mx) { return a < mn ? mn : (a > mx ? mx : a); } + + inline float vdistsqr(const float* a, const float* b) { return sqr(b[0]-a[0]) + sqr(b[1]-a[1]); } + inline float vdist(const float* a, const float* b) { return sqrtf(vdistsqr(a,b)); } + inline void vcpy(float* a, const float* b) { a[0]=b[0]; a[1]=b[1]; } + inline float vdot(const float* a, const float* b) { return a[0]*b[0] + a[1]*b[1]; } + inline float vperp(const float* a, const float* b) { return a[0]*b[1] - a[1]*b[0]; } + inline void vsub(float* v, const float* a, const float* b) { v[0] = a[0]-b[0]; v[1] = a[1]-b[1]; } + inline void vadd(float* v, const float* a, const float* b) { v[0] = a[0]+b[0]; v[1] = a[1]+b[1]; } + inline void vscale(float* v, const float* a, const float s) { v[0] = a[0]*s; v[1] = a[1]*s; } + inline void vset(float* v, float x, float y) { v[0]=x; v[1]=y; } + inline float vlensqr(const float* v) { return vdot(v,v); } + inline float vlen(const float* v) { return sqrtf(vlensqr(v)); } + 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); } + 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; } + inline void vnorm(float* v) + { + float d = vlen(v); + if (d > 0.0001f) + { + d = 1.0f/d; + v[0] *= d; + v[1] *= d; + } + } +} +inline float triarea(const float* a, const float* b, const float* c) +{ + 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]); +} + +static void closestPtPtSeg(const float* pt, + const float* sp, const float* sq, + float& t) +{ + float dir[2],diff[3]; + vsub(dir,sq,sp); + vsub(diff,pt,sp); + t = vdot(diff,dir); + if (t <= 0.0f) { t = 0; return; } + float d = vdot(dir,dir); + if (t >= d) { t = 1; return; } + t /= d; +} + +static float distPtSegSqr(const float* pt, const float* sp, const float* sq) +{ + float t; + closestPtPtSeg(pt, sp,sq, t); + float np[2]; + vlerp(np, sp,sq, t); + return vdistsqr(pt,np); +} + static int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v, const MT_Vector3& pos1, const MT_Scalar r1, float& tmin, float& tmax) @@ -152,20 +209,6 @@ return false; } -static float interpolateToi(float a, const float* dir, const float* toi, const int ntoi) -{ - for (int i = 0; i < ntoi; ++i) - { - int next = (i+1) % ntoi; - float t; - if (inBetweenAngle(a, dir[i], dir[next], t)) - { - return lerp(toi[i], toi[next], t); - } - } - return 0; -} - KX_ObstacleSimulation::KX_ObstacleSimulation(MT_Scalar levelHeight, bool enableVisualization) : m_levelHeight(levelHeight) , m_enableVisualization(enableVisualization) @@ -186,6 +229,15 @@ { KX_Obstacle* obstacle = new KX_Obstacle(); obstacle->m_gameObj = gameobj; + + vset(obstacle->vel, 0,0); + vset(obstacle->pvel, 0,0); + vset(obstacle->dvel, 0,0); + vset(obstacle->nvel, 0,0); + for (int i = 0; i < VEL_HIST_SIZE; ++i) + vset(&obstacle->hvel[i*2], 0,0); + obstacle->hhead = 0; + gameobj->RegisterObstacle(this); m_obstacles.push_back(obstacle); return obstacle; @@ -222,7 +274,6 @@ obstacle->m_pos = MT_Point3(vj[0], vj[2], vj[1]); obstacle->m_pos2 = MT_Point3(vi[0], vi[2], vi[1]); obstacle->m_rad = 0; - obstacle->m_vel = MT_Vector2(0,0); } } } @@ -254,8 +305,16 @@ KX_Obstacle* obs = m_obstacles[i]; obs->m_pos = obs->m_gameObj->NodeGetWorldPosition(); - obs->m_vel.x() = obs->m_gameObj->GetLinearVelocity().x(); - obs->m_vel.y() = obs->m_gameObj->GetLinearVelocity().y(); + obs->vel[0] = obs->m_gameObj->GetLinearVelocity().x(); + obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y(); + + // Update velocity history and calculate perceived (average) velocity. + vcpy(&obs->hvel[obs->hhead*2], obs->vel); + obs->hhead = (obs->hhead+1) % VEL_HIST_SIZE; + vset(obs->pvel,0,0); + for (int j = 0; j < VEL_HIST_SIZE; ++j) + vadd(obs->pvel, obs->pvel, &obs->hvel[j*2]); + vscale(obs->pvel, obs->pvel, 1.0f/VEL_HIST_SIZE); } } @@ -329,7 +388,8 @@ } } -bool KX_ObstacleSimulation::FilterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst) +static bool filterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst, + float levelHeight) { //filter obstacles by type if ( (otherObst == activeObst) || @@ -338,7 +398,7 @@ //filter obstacles by position MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst); - if ( fabs(activeObst->m_pos.z() - p.z()) > m_levelHeight) + if ( fabs(activeObst->m_pos.z() - p.z()) > levelHeight) return false; return true; @@ -373,71 +433,100 @@ return obstacle; } -void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, - MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle) +static const float VEL_WEIGHT = 2.0f; +static const float CUR_VEL_WEIGHT = 0.75f; +static const float SIDE_WEIGHT = 0.75f; +static const float TOI_WEIGHT = 2.5f; + +static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, + KX_Obstacles& obstacles, float levelHeight, const float vmax, @@ Diff output truncated at 10240 characters. @@ _______________________________________________ Bf-blender-cvs mailing list Bf-blender-cvs@blender.org http://lists.blender.org/mailman/listinfo/bf-blender-cvs