[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [31050] branches/soc-2010-nicks: integrated adaptive sampling algorithm for obstacle avoidance
Nick Samarin
nicks1987 at bigmir.net
Wed Aug 4 21:32:37 CEST 2010
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\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"
+ 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\rasterizer;..\..\..\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. @@
More information about the Bf-blender-cvs
mailing list