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. @@ _______________________________________________ Bf-blender-cvs mailing list Bf-blender-cvs@blender.org http://lists.blender.org/mailman/listinfo/bf-blender-cvs