[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [31222] branches/soc-2010-nicks/source: reworked obstacle simulation in order to have two realizations: with "cell " and "ray" sampling
Nick Samarin
nicks1987 at bigmir.net
Tue Aug 10 22:48:28 CEST 2010
Revision: 31222
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=31222
Author: nicks
Date: 2010-08-10 22:48:28 +0200 (Tue, 10 Aug 2010)
Log Message:
-----------
reworked obstacle simulation in order to have two realizations: with "cell" and "ray" sampling
Modified Paths:
--------------
branches/soc-2010-nicks/source/blender/editors/util/navmesh_conversion.cpp
branches/soc-2010-nicks/source/blender/makesdna/DNA_scene_types.h
branches/soc-2010-nicks/source/blender/makesrna/intern/rna_scene.c
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_Scene.cpp
Modified: branches/soc-2010-nicks/source/blender/editors/util/navmesh_conversion.cpp
===================================================================
--- branches/soc-2010-nicks/source/blender/editors/util/navmesh_conversion.cpp 2010-08-10 20:33:15 UTC (rev 31221)
+++ branches/soc-2010-nicks/source/blender/editors/util/navmesh_conversion.cpp 2010-08-10 20:48:28 UTC (rev 31222)
@@ -341,7 +341,7 @@
{
memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
}
- //create new recast data corresponded to dtris and renumber for continious indices
+ //create new recast data corresponded to dtris and renumber for continuous indices
int prevPolyIdx=-1, curPolyIdx, newPolyIdx=0;
dtrisToPolysMap = new int[ndtris];
for (int i=0; i<ndtris; i++)
Modified: branches/soc-2010-nicks/source/blender/makesdna/DNA_scene_types.h
===================================================================
--- branches/soc-2010-nicks/source/blender/makesdna/DNA_scene_types.h 2010-08-10 20:33:15 UTC (rev 31221)
+++ branches/soc-2010-nicks/source/blender/makesdna/DNA_scene_types.h 2010-08-10 20:48:28 UTC (rev 31222)
@@ -500,7 +500,8 @@
/* obstacleSimulation */
#define OBSTSIMULATION_NONE 0
-#define OBSTSIMULATION_TOI 1
+#define OBSTSIMULATION_TOI_rays 1
+#define OBSTSIMULATION_TOI_cells 2
/* GameData.flag */
#define GAME_ENABLE_ALL_FRAMES (1 << 1)
Modified: branches/soc-2010-nicks/source/blender/makesrna/intern/rna_scene.c
===================================================================
--- branches/soc-2010-nicks/source/blender/makesrna/intern/rna_scene.c 2010-08-10 20:33:15 UTC (rev 31221)
+++ branches/soc-2010-nicks/source/blender/makesrna/intern/rna_scene.c 2010-08-10 20:48:28 UTC (rev 31222)
@@ -1637,7 +1637,8 @@
static EnumPropertyItem obstacle_simulation_items[] = {
{OBSTSIMULATION_NONE, "NONE", 0, "None", ""},
- {OBSTSIMULATION_TOI, "RVO", 0, "RVO", ""},
+ {OBSTSIMULATION_TOI_rays, "RVO (rays)", 0, "RVO (rays)", ""},
+ {OBSTSIMULATION_TOI_cells, "RVO (cells)", 0, "RVO (cells)", ""},
{0, NULL, 0, NULL, NULL}};
srna= RNA_def_struct(brna, "SceneGameData", NULL);
Modified: branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp
===================================================================
--- branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp 2010-08-10 20:33:15 UTC (rev 31221)
+++ branches/soc-2010-nicks/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp 2010-08-10 20:48:28 UTC (rev 31222)
@@ -209,6 +209,20 @@
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)
@@ -404,52 +418,221 @@
return true;
}
-KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI(MT_Scalar levelHeight, bool enableVisualization):
- KX_ObstacleSimulation(levelHeight, enableVisualization),
- m_avoidSteps(32),
- m_minToi(0.5f),
- m_maxToi(1.2f),
- m_angleWeight(4.0f),
+///////////*********TOI_rays**********/////////////////
+KX_ObstacleSimulationTOI::KX_ObstacleSimulationTOI(MT_Scalar levelHeight, bool enableVisualization)
+: KX_ObstacleSimulation(levelHeight, enableVisualization),
+ m_maxSamples(32),
+ m_minToi(0.0f),
+ m_maxToi(0.0f),
+ m_velWeight(1.0f),
+ m_curVelWeight(1.0f),
m_toiWeight(1.0f),
- m_collisionWeight(100.0f)
+ m_collisionWeight(1.0f)
{
-
}
-KX_ObstacleSimulationTOI::~KX_ObstacleSimulationTOI()
+
+void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
+ MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle)
{
- for (size_t i=0; i<m_toiCircles.size(); i++)
- {
- TOICircle* toi = m_toiCircles[i];
- delete toi;
- }
- m_toiCircles.clear();
+ int nobs = m_obstacles.size();
+ int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
+ if (obstidx == nobs)
+ return;
+
+ vset(activeObst->dvel, velocity.x(), velocity.y());
+
+ //apply RVO
+ sampleRVO(activeObst, activeNavMeshObj, maxDeltaAngle);
+
+ // Fake dynamic constraint.
+ float dv[2];
+ float vel[2];
+ vsub(dv, activeObst->nvel, activeObst->vel);
+ float ds = vlen(dv);
+ if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
+ vscale(dv, dv, fabs(maxDeltaSpeed/ds));
+ vadd(vel, activeObst->vel, dv);
+
+ velocity.x() = vel[0];
+ velocity.y() = vel[1];
}
-KX_Obstacle* KX_ObstacleSimulationTOI::CreateObstacle(KX_GameObject* gameobj)
+///////////*********TOI_rays**********/////////////////
+static const int AVOID_MAX_STEPS = 128;
+struct TOICircle
{
- KX_Obstacle* obstacle = KX_ObstacleSimulation::CreateObstacle(gameobj);
- m_toiCircles.push_back(new TOICircle());
- return obstacle;
+ TOICircle() : n(0), minToi(0), maxToi(1) {}
+ float toi[AVOID_MAX_STEPS]; // Time of impact (seconds)
+ float toie[AVOID_MAX_STEPS]; // Time of exit (seconds)
+ float dir[AVOID_MAX_STEPS]; // Direction (radians)
+ int n; // Number of samples
+ float minToi, maxToi; // Min/max TOI (seconds)
+};
+
+KX_ObstacleSimulationTOI_rays::KX_ObstacleSimulationTOI_rays(MT_Scalar levelHeight, bool enableVisualization):
+ KX_ObstacleSimulationTOI(levelHeight, enableVisualization)
+{
+ m_maxSamples = 32;
+ m_minToi = 0.5f;
+ m_maxToi = 1.2f;
+ m_velWeight = 4.0f;
+ m_toiWeight = 1.0f;
+ m_collisionWeight = 100.0f;
}
-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;
+void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
+ const float maxDeltaAngle)
+{
+ MT_Vector2 vel(activeObst->dvel[0], activeObst->dvel[1]);
+ float vmax = (float) vel.length();
+ float odir = (float) atan2(vel.y(), vel.x());
+
+ MT_Vector2 ddir = vel;
+ ddir.normalize();
+
+ float bestScore = FLT_MAX;
+ float bestDir = odir;
+ float bestToi = 0;
+
+ TOICircle tc;
+ tc.n = m_maxSamples;
+ tc.minToi = m_minToi;
+ tc.maxToi = m_maxToi;
+
+ const int iforw = m_maxSamples/2;
+ const float aoff = (float)iforw / (float)m_maxSamples;
+
+ size_t nobs = m_obstacles.size();
+ for (int iter = 0; iter < m_maxSamples; ++iter)
+ {
+ // Calculate sample velocity
+ const float ndir = ((float)iter/(float)m_maxSamples) - aoff;
+ const float dir = odir+ndir*M_PI*2;
+ MT_Vector2 svel;
+ svel.x() = cosf(dir) * vmax;
+ svel.y() = sinf(dir) * vmax;
+
+ // Find min time of impact and exit amongst all obstacles.
+ float tmin = m_maxToi;
+ float tmine = 0;
+ for (int i = 0; i < nobs; ++i)
+ {
+ KX_Obstacle* ob = m_obstacles[i];
+ bool res = filterObstacle(activeObst, activeNavMeshObj, ob, m_levelHeight);
+ if (!res)
+ continue;
+
+ float htmin,htmax;
+
+ if (ob->m_shape == KX_OBSTACLE_CIRCLE)
+ {
+ MT_Vector2 vab;
+ if (vlen(ob->vel) < 0.01f*0.01f)
+ {
+ // Stationary, use VO
+ vab = svel;
+ }
+ else
+ {
+ // Moving, use RVO
+ vab = 2*svel - vel - ob->vel;
+ }
+
+ if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad,
+ vab, ob->m_pos, ob->m_rad, htmin, htmax))
+ continue;
+ }
+ else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
+ {
+ MT_Point3 p1 = ob->m_pos;
+ MT_Point3 p2 = ob->m_pos2;
+ //apply world transform
+ if (ob->m_type == KX_OBSTACLE_NAV_MESH)
+ {
+ KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
+ p1 = navmeshobj->TransformToWorldCoords(p1);
+ p2 = navmeshobj->TransformToWorldCoords(p2);
+ }
+ if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel,
+ p1, p2, ob->m_rad, htmin, htmax))
+ continue;
+ }
+
+ if (htmin > 0.0f)
+ {
+ // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
+ if (htmin < tmin)
+ tmin = htmin;
+ }
+ else if (htmax > 0.0f)
+ {
+ // The agent overlaps the obstacle, keep track of first safe exit.
+ if (htmax > tmine)
+ tmine = htmax;
+ }
+ }
+
+ // Calculate sample penalties and final score.
+ const float apen = m_velWeight * fabsf(ndir);
+ const float tpen = m_toiWeight * (1.0f/(0.0001f+tmin/m_maxToi));
+ const float cpen = m_collisionWeight * (tmine/m_minToi)*(tmine/m_minToi);
+ const float score = apen + tpen + cpen;
+
+ // Update best score.
+ if (score < bestScore)
+ {
+ bestDir = dir;
+ bestToi = tmin;
+ bestScore = score;
+ }
+
+ tc.dir[iter] = dir;
+ tc.toi[iter] = tmin;
+ tc.toie[iter] = tmine;
+ }
+
+ if (vlen(activeObst->vel) > 0.1)
+ {
+ // Constrain max turn rate.
+ float cura = atan2(activeObst->vel[1],activeObst->vel[0]);
+ float da = bestDir - cura;
+ if (da < -M_PI) da += (float)M_PI*2;
+ if (da > M_PI) da -= (float)M_PI*2;
+ if (da < -maxDeltaAngle)
+ {
+ bestDir = cura - maxDeltaAngle;
+ bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
+ }
+ else if (da > maxDeltaAngle)
+ {
+ bestDir = cura + maxDeltaAngle;
+ bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
+ }
+ }
+
+ // Adjust speed when time of impact is less than min TOI.
+ if (bestToi < m_minToi)
+ vmax *= bestToi/m_minToi;
+
+ // New steering velocity.
+ activeObst->nvel[0] = cosf(bestDir) * vmax;
+ activeObst->nvel[1] = sinf(bestDir) * vmax;
+}
+
+///////////********* TOI_cells**********/////////////////
+
static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
KX_Obstacles& obstacles, float levelHeight, const float vmax,
- const float* spos, const float cs, const int nspos,
- float* res)
+ const float* spos, const float cs, const int nspos, float* res,
+ float maxToi, float velWeight, float curVelWeight, float sideWeight,
+ float toiWeight)
{
vset(res, 0,0);
const float ivmax = 1.0f / vmax;
- // Max time of collision to be considered.
- const float maxToi = 1.5f;
-
float adir[2], adist;
vcpy(adir, activeObst->pvel);
if (vlen(adir) > 0.01f)
@@ -583,10 +766,10 @@
if (nside)
side /= nside;
- const float vpen = VEL_WEIGHT * (vdist(vcand, activeObst->dvel) * ivmax);
- const float vcpen = CUR_VEL_WEIGHT * (vdist(vcand, activeObst->vel) * ivmax);
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list