diff options
Diffstat (limited to 'source/gameengine/Ketsji/KX_ObstacleSimulation.cpp')
| -rw-r--r-- | source/gameengine/Ketsji/KX_ObstacleSimulation.cpp | 832 |
1 files changed, 0 insertions, 832 deletions
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp deleted file mode 100644 index 20d4a517552..00000000000 --- a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp +++ /dev/null @@ -1,832 +0,0 @@ -/* - * ***** BEGIN GPL LICENSE BLOCK ***** - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU General Public License - * as published by the Free Software Foundation; either version 2 - * of the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software Foundation, - * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. - * - * Contributor(s): none yet. - * - * ***** END GPL LICENSE BLOCK ***** - */ - -/** \file gameengine/Ketsji/KX_ObstacleSimulation.cpp - * \ingroup ketsji - * - * Simulation for obstacle avoidance behavior - */ - -#include "KX_ObstacleSimulation.h" -#include "KX_NavMeshObject.h" -#include "KX_PythonInit.h" -#include "DNA_object_types.h" -#include "BLI_math.h" - -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 void vset(float v[2], float x, float y) { v[0] = x; v[1] = y; } -} - -/* grr, seems moto provides no nice way to do this */ -#define MT_3D_AS_2D(v) MT_Vector2((v)[0], (v)[1]) - -static int sweepCircleCircle( - const MT_Vector2 &pos0, const MT_Scalar r0, const MT_Vector2 &v, - const MT_Vector2 &pos1, const MT_Scalar r1, - float& tmin, float& tmax) -{ - static const float EPS = 0.0001f; - MT_Vector2 c0(pos0.x(), pos0.y()); - MT_Vector2 c1(pos1.x(), pos1.y()); - MT_Vector2 s = c1 - c0; - MT_Scalar r = r0+r1; - float c = s.length2() - r*r; - float a = v.length2(); - if (a < EPS) return 0; // not moving - - // Overlap, calc time to exit. - float b = MT_dot(v,s); - float d = b*b - a*c; - if (d < 0.0f) return 0; // no intersection. - tmin = (b - sqrtf(d)) / a; - tmax = (b + sqrtf(d)) / a; - return 1; -} - -static int sweepCircleSegment( - const MT_Vector2 &pos0, const MT_Scalar r0, const MT_Vector2 &v, - const MT_Vector2& pa, const MT_Vector2 &pb, const MT_Scalar sr, - float& tmin, float &tmax) -{ - // equation parameters - MT_Vector2 c0(pos0.x(), pos0.y()); - MT_Vector2 sa(pa.x(), pa.y()); - MT_Vector2 sb(pb.x(), pb.y()); - MT_Vector2 L = sb-sa; - MT_Vector2 H = c0-sa; - MT_Scalar radius = r0+sr; - float l2 = L.length2(); - float r2 = radius * radius; - float dl = perp(v, L); - float hl = perp(H, L); - float a = dl * dl; - float b = 2.0f * hl * dl; - float c = hl * hl - (r2 * l2); - float d = (b*b) - (4.0f * a * c); - - // infinite line missed by infinite ray. - if (d < 0.0f) - return 0; - - d = sqrtf(d); - tmin = (-b - d) / (2.0f * a); - tmax = (-b + d) / (2.0f * a); - - // line missed by ray range. - /* if (tmax < 0.0f || tmin > 1.0f) - return 0;*/ - - // find what part of the ray was collided. - MT_Vector2 Pedge; - Pedge = c0+v*tmin; - H = Pedge - sa; - float e0 = MT_dot(H, L) / l2; - Pedge = c0 + v*tmax; - H = Pedge - sa; - float e1 = MT_dot(H, L) / l2; - - if (e0 < 0.0f || e1 < 0.0f) - { - float ctmin, ctmax; - if (sweepCircleCircle(pos0, r0, v, pa, sr, ctmin, ctmax)) - { - if (e0 < 0.0f && ctmin > tmin) - tmin = ctmin; - if (e1 < 0.0f && ctmax < tmax) - tmax = ctmax; - } - else - { - return 0; - } - } - - if (e0 > 1.0f || e1 > 1.0f) - { - float ctmin, ctmax; - if (sweepCircleCircle(pos0, r0, v, pb, sr, ctmin, ctmax)) - { - if (e0 > 1.0f && ctmin > tmin) - tmin = ctmin; - if (e1 > 1.0f && ctmax < tmax) - tmax = ctmax; - } - else - { - return 0; - } - } - - return 1; -} - -static bool inBetweenAngle(float a, float amin, float amax, float& t) -{ - if (amax < amin) amax += (float)M_PI*2; - if (a < amin-(float)M_PI) a += (float)M_PI*2; - if (a > amin+(float)M_PI) a -= (float)M_PI*2; - if (a >= amin && a < amax) - { - t = (a-amin) / (amax-amin); - return true; - } - 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) -{ - -} - -KX_ObstacleSimulation::~KX_ObstacleSimulation() -{ - for (size_t i=0; i<m_obstacles.size(); i++) - { - KX_Obstacle* obs = m_obstacles[i]; - delete obs; - } - m_obstacles.clear(); -} -KX_Obstacle* KX_ObstacleSimulation::CreateObstacle(KX_GameObject* gameobj) -{ - 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; -} - -void KX_ObstacleSimulation::AddObstacleForObj(KX_GameObject* gameobj) -{ - KX_Obstacle* obstacle = CreateObstacle(gameobj); - struct Object* blenderobject = gameobj->GetBlenderObject(); - obstacle->m_type = KX_OBSTACLE_OBJ; - obstacle->m_shape = KX_OBSTACLE_CIRCLE; - obstacle->m_rad = blenderobject->obstacleRad; -} - -void KX_ObstacleSimulation::AddObstaclesForNavMesh(KX_NavMeshObject* navmeshobj) -{ - dtStatNavMesh* navmesh = navmeshobj->GetNavMesh(); - if (navmesh) - { - int npoly = navmesh->getPolyCount(); - for (int pi=0; pi<npoly; pi++) - { - const dtStatPoly* poly = navmesh->getPoly(pi); - - for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++) - { - if (poly->n[j]) continue; - const float* vj = navmesh->getVertex(poly->v[j]); - const float* vi = navmesh->getVertex(poly->v[i]); - - KX_Obstacle* obstacle = CreateObstacle(navmeshobj); - obstacle->m_type = KX_OBSTACLE_NAV_MESH; - obstacle->m_shape = KX_OBSTACLE_SEGMENT; - 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; - } - } - } -} - -void KX_ObstacleSimulation::DestroyObstacleForObj(KX_GameObject* gameobj) -{ - for (size_t i=0; i<m_obstacles.size(); ) - { - if (m_obstacles[i]->m_gameObj == gameobj) - { - KX_Obstacle* obstacle = m_obstacles[i]; - obstacle->m_gameObj->UnregisterObstacle(); - m_obstacles[i] = m_obstacles.back(); - m_obstacles.pop_back(); - delete obstacle; - } - else - i++; - } -} - -void KX_ObstacleSimulation::UpdateObstacles() -{ - for (size_t i=0; i<m_obstacles.size(); i++) - { - if (m_obstacles[i]->m_type==KX_OBSTACLE_NAV_MESH || m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT) - continue; - - KX_Obstacle* obs = m_obstacles[i]; - obs->m_pos = obs->m_gameObj->NodeGetWorldPosition(); - obs->vel[0] = obs->m_gameObj->GetLinearVelocity().x(); - obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y(); - - // Update velocity history and calculate perceived (average) velocity. - copy_v2_v2(&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) - add_v2_v2v2(obs->pvel, obs->pvel, &obs->hvel[j * 2]); - mul_v2_fl(obs->pvel, 1.0f / VEL_HIST_SIZE); - } -} - -KX_Obstacle* KX_ObstacleSimulation::GetObstacle(KX_GameObject* gameobj) -{ - for (size_t i=0; i<m_obstacles.size(); i++) - { - if (m_obstacles[i]->m_gameObj == gameobj) - return m_obstacles[i]; - } - - return NULL; -} - -void KX_ObstacleSimulation::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, - MT_Vector3& velocity, MT_Scalar maxDeltaSpeed,MT_Scalar maxDeltaAngle) -{ -} - -void KX_ObstacleSimulation::DrawObstacles() -{ - if (!m_enableVisualization) - return; - static const MT_Vector3 bluecolor(0,0,1); - static const MT_Vector3 normal(0.0f, 0.0f, 1.0f); - static const int SECTORS_NUM = 32; - for (size_t i=0; i<m_obstacles.size(); i++) - { - if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT) - { - MT_Point3 p1 = m_obstacles[i]->m_pos; - MT_Point3 p2 = m_obstacles[i]->m_pos2; - //apply world transform - if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH) - { - KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(m_obstacles[i]->m_gameObj); - p1 = navmeshobj->TransformToWorldCoords(p1); - p2 = navmeshobj->TransformToWorldCoords(p2); - } - - KX_RasterizerDrawDebugLine(p1, p2, bluecolor); - } - else if (m_obstacles[i]->m_shape==KX_OBSTACLE_CIRCLE) - { - KX_RasterizerDrawDebugCircle(m_obstacles[i]->m_pos, m_obstacles[i]->m_rad, bluecolor, - normal, SECTORS_NUM); - } - } -} - -static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle) -{ - switch (obstacle->m_shape) - { - case KX_OBSTACLE_SEGMENT : - { - MT_Vector3 ab = obstacle->m_pos2 - obstacle->m_pos; - if (!ab.fuzzyZero()) - { - const MT_Scalar dist = ab.length(); - MT_Vector3 abdir = ab.normalized(); - MT_Vector3 v = pos - obstacle->m_pos; - MT_Scalar proj = abdir.dot(v); - CLAMP(proj, 0, dist); - MT_Point3 res = obstacle->m_pos + abdir*proj; - return res; - } - ATTR_FALLTHROUGH; - } - case KX_OBSTACLE_CIRCLE : - default: - return obstacle->m_pos; - } -} - -static bool filterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst, - float levelHeight) -{ - //filter obstacles by type - if ( (otherObst == activeObst) || - (otherObst->m_type==KX_OBSTACLE_NAV_MESH && otherObst->m_gameObj!=activeNavMeshObj) ) - return false; - - //filter obstacles by position - MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst); - if ( fabsf(activeObst->m_pos.z() - p.z()) > levelHeight) - return false; - - return true; -} - -///////////*********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(1.0f) -{ -} - - -void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, - MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle) -{ - 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]; - sub_v2_v2v2(dv, activeObst->nvel, activeObst->vel); - float ds = len_v2(dv); - if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed) - mul_v2_fl(dv, fabs(maxDeltaSpeed / ds)); - add_v2_v2v2(vel, activeObst->vel, dv); - - velocity.x() = vel[0]; - velocity.y() = vel[1]; -} - -///////////*********TOI_rays**********///////////////// -static const int AVOID_MAX_STEPS = 128; -struct TOICircle -{ - 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; -} - - -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*(float)M_PI*2.0f; - 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.0f; - 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 (len_v2(ob->vel) < 0.01f * 0.01f) { - // Stationary, use VO - vab = svel; - } - else - { - // Moving, use RVO - vab = 2*svel - vel - ob->vel; - } - - if (!sweepCircleCircle(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad, - vab, MT_3D_AS_2D(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(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad, svel, - MT_3D_AS_2D(p1), MT_3D_AS_2D(p2), ob->m_rad, htmin, htmax)) - { - continue; - } - } - else { - 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 (len_v2(activeObst->vel) > 0.1f) { - // 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, - float maxToi, float velWeight, float curVelWeight, float sideWeight, - float toiWeight) -{ - vset(res, 0,0); - - const float ivmax = 1.0f / vmax; - - float adir[2] /*, adist */; - if (normalize_v2_v2(adir, activeObst->pvel) <= 0.01f) { - zero_v2(adir); - } - - float activeObstPos[2]; - vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y()); - /* adist = vdot(adir, activeObstPos); */ - - float minPenalty = FLT_MAX; - - for (int n = 0; n < nspos; ++n) - { - float vcand[2]; - copy_v2_v2(vcand, &spos[n * 2]); - - // Find min time of impact and exit amongst all obstacles. - float tmin = maxToi; - float side = 0; - int nside = 0; - - for (int i = 0; i < obstacles.size(); ++i) - { - KX_Obstacle* ob = obstacles[i]; - bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight); - if (!res) - continue; - float htmin, htmax; - - if (ob->m_shape==KX_OBSTACLE_CIRCLE) - { - float vab[2]; - - // Moving, use RVO - mul_v2_v2fl(vab, vcand, 2); - sub_v2_v2v2(vab, vab, activeObst->vel); - sub_v2_v2v2(vab, vab, ob->vel); - - // Side - // NOTE: dp, and dv are constant over the whole calculation, - // they can be precomputed per object. - const float* pa = activeObstPos; - float pb[2]; - vset(pb, ob->m_pos.x(), ob->m_pos.y()); - - const float orig[2] = {0, 0}; - float dp[2], dv[2], np[2]; - sub_v2_v2v2(dp, pb, pa); - normalize_v2(dp); - sub_v2_v2v2(dv, ob->dvel, activeObst->dvel); - - /* TODO: use line_point_side_v2 */ - if (area_tri_signed_v2(orig, dp, dv) < 0.01f) { - np[0] = -dp[1]; - np[1] = dp[0]; - } - else { - np[0] = dp[1]; - np[1] = -dp[0]; - } - - side += clamp(min(dot_v2v2(dp, vab), - dot_v2v2(np, vab)) * 2.0f, 0.0f, 1.0f); - nside++; - - if (!sweepCircleCircle(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad, - vab, MT_3D_AS_2D(ob->m_pos), ob->m_rad, htmin, htmax)) - { - continue; - } - - // Handle overlapping obstacles. - if (htmin < 0.0f && htmax > 0.0f) - { - // Avoid more when overlapped. - htmin = -htmin * 0.5f; - } - } - 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); - } - float p[2], q[2]; - vset(p, p1.x(), p1.y()); - vset(q, p2.x(), p2.y()); - - // NOTE: the segments are assumed to come from a navmesh which is shrunken by - // the agent radius, hence the use of really small radius. - // This can be handle more efficiently by using seg-seg test instead. - // If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f! - const float r = 0.01f; // agent->rad - if (dist_squared_to_line_segment_v2(activeObstPos, p, q) < sqr(r + ob->m_rad)) { - float sdir[2], snorm[2]; - sub_v2_v2v2(sdir, q, p); - snorm[0] = sdir[1]; - snorm[1] = -sdir[0]; - // If the velocity is pointing towards the segment, no collision. - if (dot_v2v2(snorm, vcand) < 0.0f) - continue; - // Else immediate collision. - htmin = 0.0f; - htmax = 10.0f; - } - else - { - if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax)) - continue; - } - - // Avoid less when facing walls. - htmin *= 2.0f; - } - else { - continue; - } - - if (htmin >= 0.0f) - { - // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. - if (htmin < tmin) - tmin = htmin; - } - } - - // Normalize side bias, to prevent it dominating too much. - if (nside) - side /= nside; - - const float vpen = velWeight * (len_v2v2(vcand, activeObst->dvel) * ivmax); - const float vcpen = curVelWeight * (len_v2v2(vcand, activeObst->vel) * ivmax); - const float spen = sideWeight * side; - const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi)); - - const float penalty = vpen + vcpen + spen + tpen; - - if (penalty < minPenalty) { - minPenalty = penalty; - copy_v2_v2(res, vcand); - } - } -} - -void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, - const float maxDeltaAngle) -{ - vset(activeObst->nvel, 0.f, 0.f); - float vmax = len_v2(activeObst->dvel); - - float* spos = new float[2*m_maxSamples]; - int nspos = 0; - - if (!m_adaptive) - { - const float cvx = activeObst->dvel[0]*m_bias; - const float cvy = activeObst->dvel[1]*m_bias; - float vmax = len_v2(activeObst->dvel); - const float vrange = vmax*(1-m_bias); - const float cs = 1.0f / (float)m_sampleRadius*vrange; - - for (int y = -m_sampleRadius; y <= m_sampleRadius; ++y) - { - for (int x = -m_sampleRadius; x <= m_sampleRadius; ++x) - { - if (nspos < m_maxSamples) - { - const float vx = cvx + (float)(x+0.5f)*cs; - const float vy = cvy + (float)(y+0.5f)*cs; - if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue; - spos[nspos*2+0] = vx; - spos[nspos*2+1] = vy; - nspos++; - } - } - } - processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2, - nspos, activeObst->nvel, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight); - } - else - { - int rad; - float res[2]; - float cs; - // First sample location. - rad = 4; - res[0] = activeObst->dvel[0]*m_bias; - res[1] = activeObst->dvel[1]*m_bias; - cs = vmax*(2-m_bias*2) / (float)(rad-1); - - for (int k = 0; k < 5; ++k) - { - const float half = (rad-1)*cs*0.5f; - - nspos = 0; - for (int y = 0; y < rad; ++y) - { - for (int x = 0; x < rad; ++x) - { - const float v_xy[2] = { - res[0] + x * cs - half, - res[1] + y * cs - half}; - - if (len_squared_v2(v_xy) > sqr(vmax + cs / 2)) - continue; - - copy_v2_v2(&spos[nspos * 2 + 0], v_xy); - nspos++; - } - } - - processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2, - nspos, res, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight); - - cs *= 0.5f; - } - copy_v2_v2(activeObst->nvel, res); - } - - delete [] spos; -} - -KX_ObstacleSimulationTOI_cells::KX_ObstacleSimulationTOI_cells(MT_Scalar levelHeight, bool enableVisualization) -: KX_ObstacleSimulationTOI(levelHeight, enableVisualization) -, m_bias(0.4f) -, m_adaptive(true) -, m_sampleRadius(15) -{ - m_maxSamples = (m_sampleRadius*2+1)*(m_sampleRadius*2+1) + 100; - m_maxToi = 1.5f; - m_velWeight = 2.0f; - m_curVelWeight = 0.75f; - m_toiWeight = 2.5f; - m_collisionWeight = 0.75f; //side_weight -} |