Page MenuHome
Differential D2457 Diff 8102 source/gameengine/Ketsji/KX_ObstacleSimulation.cpp

Changeset View

Standalone View

View Options

source/gameengine/Ketsji/KX_ObstacleSimulation.cpp

Context not available.
#include "KX_ObstacleSimulation.h" #include "KX_ObstacleSimulation.h"
#include "KX_NavMeshObject.h" #include "KX_NavMeshObject.h"
#include "KX_PythonInit.h" #include "KX_Globals.h"
#include "DNA_object_types.h" #include "DNA_object_types.h"
#include "BLI_math.h" #include "BLI_math.h"
Context not available.
inline void vset(float v[2], float x, float y) { v[0] = x; v[1] = y; } 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( static int sweepCircleCircle(
const MT_Vector2 &pos0, const MT_Scalar r0, const MT_Vector2 &v, const MT_Vector2 &pos0, const MT_Scalar r0, const MT_Vector2 &v,
const MT_Vector2 &pos1, const MT_Scalar r1, const MT_Vector2 &pos1, const MT_Scalar r1,
Context not available.
vset(&obstacle->hvel[i*2], 0,0); vset(&obstacle->hvel[i*2], 0,0);
obstacle->hhead = 0; obstacle->hhead = 0;
gameobj->RegisterObstacle(this);
m_obstacles.push_back(obstacle); m_obstacles.push_back(obstacle);
return obstacle; return obstacle;
} }
Context not available.
KX_Obstacle* obstacle = CreateObstacle(navmeshobj); KX_Obstacle* obstacle = CreateObstacle(navmeshobj);
obstacle->m_type = KX_OBSTACLE_NAV_MESH; obstacle->m_type = KX_OBSTACLE_NAV_MESH;
obstacle->m_shape = KX_OBSTACLE_SEGMENT; obstacle->m_shape = KX_OBSTACLE_SEGMENT;
obstacle->m_pos = MT_Point3(vj[0], vj[2], vj[1]); obstacle->m_pos = MT_Vector3(vj[0], vj[2], vj[1]);
obstacle->m_pos2 = MT_Point3(vi[0], vi[2], vi[1]); obstacle->m_pos2 = MT_Vector3(vi[0], vi[2], vi[1]);
obstacle->m_rad = 0; obstacle->m_rad = 0;
} }
} }
Context not available.
if (m_obstacles[i]->m_gameObj == gameobj) if (m_obstacles[i]->m_gameObj == gameobj)
{ {
KX_Obstacle* obstacle = m_obstacles[i]; KX_Obstacle* obstacle = m_obstacles[i];
obstacle->m_gameObj->UnregisterObstacle();
m_obstacles[i] = m_obstacles.back(); m_obstacles[i] = m_obstacles.back();
m_obstacles.pop_back(); m_obstacles.pop_back();
delete obstacle; delete obstacle;
Context not available.
{ {
if (!m_enableVisualization) if (!m_enableVisualization)
return; return;
static const MT_Vector3 bluecolor(0,0,1); static const MT_Vector4 bluecolor(0.0f, 0.0f, 1.0f, 1.0f);
static const MT_Vector3 normal(0.0f, 0.0f, 1.0f); static const MT_Vector3 normal(0.0f, 0.0f, 1.0f);
static const int SECTORS_NUM = 32; static const int SECTORS_NUM = 32;
for (size_t i=0; i<m_obstacles.size(); i++) for (size_t i=0; i<m_obstacles.size(); i++)
{ {
if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT) if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
{ {
MT_Point3 p1 = m_obstacles[i]->m_pos; MT_Vector3 p1 = m_obstacles[i]->m_pos;
MT_Point3 p2 = m_obstacles[i]->m_pos2; MT_Vector3 p2 = m_obstacles[i]->m_pos2;
//apply world transform //apply world transform
if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH) if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH)
{ {
Context not available.
} }
} }
static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle) static MT_Vector3 nearestPointToObstacle(MT_Vector3& pos ,KX_Obstacle* obstacle)
{ {
switch (obstacle->m_shape) switch (obstacle->m_shape)
{ {
Context not available.
MT_Vector3 v = pos - obstacle->m_pos; MT_Vector3 v = pos - obstacle->m_pos;
MT_Scalar proj = abdir.dot(v); MT_Scalar proj = abdir.dot(v);
CLAMP(proj, 0, dist); CLAMP(proj, 0, dist);
MT_Point3 res = obstacle->m_pos + abdir*proj; MT_Vector3 res = obstacle->m_pos + abdir*proj;
return res; return res;
} }
} }
Context not available.
return false; return false;
//filter obstacles by position //filter obstacles by position
MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst); MT_Vector3 p = nearestPointToObstacle(activeObst->m_pos, otherObst);
if ( fabsf(activeObst->m_pos.z() - p.z()) > levelHeight) if ( fabsf(activeObst->m_pos.z() - p.z()) > levelHeight)
return false; return false;
Context not available.
else else
{ {
// Moving, use RVO // Moving, use RVO
vab = 2*svel - vel - ob->vel; vab = 2*svel - vel - MT_Vector2(ob->vel);
} }
if (!sweepCircleCircle(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad, if (!sweepCircleCircle(activeObst->m_pos.to2d(), activeObst->m_rad,
vab, MT_3D_AS_2D(ob->m_pos), ob->m_rad, htmin, htmax)) vab, ob->m_pos.to2d(), ob->m_rad, htmin, htmax))
{ {
continue; continue;
} }
} }
else if (ob->m_shape == KX_OBSTACLE_SEGMENT) else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
{ {
MT_Point3 p1 = ob->m_pos; MT_Vector3 p1 = ob->m_pos;
MT_Point3 p2 = ob->m_pos2; MT_Vector3 p2 = ob->m_pos2;
//apply world transform //apply world transform
if (ob->m_type == KX_OBSTACLE_NAV_MESH) if (ob->m_type == KX_OBSTACLE_NAV_MESH)
{ {
Context not available.
p2 = navmeshobj->TransformToWorldCoords(p2); p2 = navmeshobj->TransformToWorldCoords(p2);
} }
if (!sweepCircleSegment(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad, svel, if (!sweepCircleSegment(activeObst->m_pos.to2d(), activeObst->m_rad, svel,
MT_3D_AS_2D(p1), MT_3D_AS_2D(p2), ob->m_rad, htmin, htmax)) p1.to2d(), p2.to2d(), ob->m_rad, htmin, htmax))
{ {
continue; continue;
} }
Context not available.
if (da < -maxDeltaAngle) if (da < -maxDeltaAngle)
{ {
bestDir = cura - maxDeltaAngle; bestDir = cura - maxDeltaAngle;
bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n)); bestToi = std::min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
} }
else if (da > maxDeltaAngle) else if (da > maxDeltaAngle)
{ {
bestDir = cura + maxDeltaAngle; bestDir = cura + maxDeltaAngle;
bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n)); bestToi = std::min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
} }
} }
Context not available.
np[1] = -dp[0]; np[1] = -dp[0];
} }
side += clamp(min(dot_v2v2(dp, vab), side += clamp(std::min(dot_v2v2(dp, vab),
dot_v2v2(np, vab)) * 2.0f, 0.0f, 1.0f); dot_v2v2(np, vab)) * 2.0f, 0.0f, 1.0f);
nside++; nside++;
if (!sweepCircleCircle(MT_3D_AS_2D(activeObst->m_pos), activeObst->m_rad, if (!sweepCircleCircle(activeObst->m_pos.to2d(), activeObst->m_rad,
vab, MT_3D_AS_2D(ob->m_pos), ob->m_rad, htmin, htmax)) MT_Vector2(vab), ob->m_pos.to2d(), ob->m_rad, htmin, htmax))
{ {
continue; continue;
} }
Context not available.
} }
else if (ob->m_shape == KX_OBSTACLE_SEGMENT) else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
{ {
MT_Point3 p1 = ob->m_pos; MT_Vector3 p1 = ob->m_pos;
MT_Point3 p2 = ob->m_pos2; MT_Vector3 p2 = ob->m_pos2;
//apply world transform //apply world transform
if (ob->m_type == KX_OBSTACLE_NAV_MESH) if (ob->m_type == KX_OBSTACLE_NAV_MESH)
{ {
Context not available.
} }
else else
{ {
if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax)) if (!sweepCircleSegment(MT_Vector2(activeObstPos), r, MT_Vector2(vcand),
MT_Vector2(p), MT_Vector2(q), ob->m_rad, htmin, htmax))
continue; continue;
} }
Context not available.