diff options
author | Campbell Barton <ideasman42@gmail.com> | 2013-08-04 07:47:43 +0400 |
---|---|---|
committer | Campbell Barton <ideasman42@gmail.com> | 2013-08-04 07:47:43 +0400 |
commit | c0b73fa1b0dd5e7bd7b8271f55d27b4d83c962e1 (patch) | |
tree | f69adecebe20e24d722985adddae645cb074d24b | |
parent | 93f5e2218a5103e91ff4c3943aec3bc3d6f9c69e (diff) |
KX_ObstacleSimulation: replace inline math functions with BLI_math functions
-rw-r--r-- | source/gameengine/Ketsji/KX_ObstacleSimulation.cpp | 161 |
1 files changed, 54 insertions, 107 deletions
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp index 8798f42fa07..acb8b7b1c8b 100644 --- a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp +++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp @@ -36,60 +36,10 @@ 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 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]; } */ /* UNUSED */ - 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; } */ /* UNUSED */ - 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); + inline void vset(float v[2], float x, float y) { v[0] = x; v[1] = y; } } static int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v, @@ -317,12 +267,12 @@ void KX_ObstacleSimulation::UpdateObstacles() obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y(); // Update velocity history and calculate perceived (average) velocity. - vcpy(&obs->hvel[obs->hhead*2], obs->vel); + 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) - vadd(obs->pvel, obs->pvel, &obs->hvel[j*2]); - vscale(obs->pvel, obs->pvel, 1.0f/VEL_HIST_SIZE); + add_v2_v2v2(obs->pvel, obs->pvel, &obs->hvel[j * 2]); + mul_v2_fl(obs->pvel, 1.0f / VEL_HIST_SIZE); } } @@ -443,11 +393,11 @@ void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, K // Fake dynamic constraint. float dv[2]; float vel[2]; - vsub(dv, activeObst->nvel, activeObst->vel); - float ds = vlen(dv); + sub_v2_v2v2(dv, activeObst->nvel, activeObst->vel); + float ds = len_v2(dv); if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed) - vscale(dv, dv, fabs(maxDeltaSpeed/ds)); - vadd(vel, activeObst->vel, dv); + mul_v2_fl(dv, fabs(maxDeltaSpeed / ds)); + add_v2_v2v2(vel, activeObst->vel, dv); velocity.x() = vel[0]; velocity.y() = vel[1]; @@ -524,8 +474,7 @@ void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMes if (ob->m_shape == KX_OBSTACLE_CIRCLE) { MT_Vector2 vab; - if (vlen(ob->vel) < 0.01f*0.01f) - { + if (len_v2(ob->vel) < 0.01f * 0.01f) { // Stationary, use VO vab = svel; } @@ -591,8 +540,7 @@ void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMes tc.toie[iter] = tmine; } - if (vlen(activeObst->vel) > 0.1) - { + if (len_v2(activeObst->vel) > 0.1f) { // Constrain max turn rate. float cura = atan2(activeObst->vel[1],activeObst->vel[0]); float da = bestDir - cura; @@ -622,21 +570,20 @@ void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMes ///////////********* 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) + 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 */; - vcpy(adir, activeObst->pvel); - if (vlen(adir) > 0.01f) - vnorm(adir); - else - vset(adir,0,0); + 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); */ @@ -646,7 +593,7 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM for (int n = 0; n < nspos; ++n) { float vcand[2]; - vcpy(vcand, &spos[n*2]); + copy_v2_v2(vcand, &spos[n * 2]); // Find min time of impact and exit amongst all obstacles. float tmin = maxToi; @@ -666,9 +613,9 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM float vab[2]; // Moving, use RVO - vscale(vab, vcand, 2); - vsub(vab, vab, activeObst->vel); - vsub(vab, vab, ob->vel); + 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, @@ -677,25 +624,24 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM 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]; - vsub(dp,pb,pa); - vnorm(dp); - vsub(dv,ob->dvel, activeObst->dvel); + 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); - const float a = triarea(orig, dp,dv); - if (a < 0.01f) - { + /* 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 - { + else { np[0] = dp[1]; np[1] = -dp[0]; } - side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f); + side += clamp(min(dot_v2v2(dp, vab), + dot_v2v2(np, vab)) * 2.0f, 0.0f, 1.0f); nside++; if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad, @@ -729,14 +675,13 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM // 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 (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad)) - { + if (dist_squared_to_line_segment_v2(activeObstPos, p, q) < sqr(r + ob->m_rad)) { float sdir[2], snorm[2]; - vsub(sdir, q, p); + 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 (vdot(snorm, vcand) < 0.0f) + if (dot_v2v2(snorm, vcand) < 0.0f) continue; // Else immediate collision. htmin = 0.0f; @@ -767,17 +712,16 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM if (nside) side /= nside; - const float vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax); - const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax); + 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) - { + if (penalty < minPenalty) { minPenalty = penalty; - vcpy(res, vcand); + copy_v2_v2(res, vcand); } } } @@ -786,7 +730,7 @@ void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMe const float maxDeltaAngle) { vset(activeObst->nvel, 0.f, 0.f); - float vmax = vlen(activeObst->dvel); + float vmax = len_v2(activeObst->dvel); float* spos = new float[2*m_maxSamples]; int nspos = 0; @@ -795,7 +739,7 @@ void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMe { const float cvx = activeObst->dvel[0]*m_bias; const float cvy = activeObst->dvel[1]*m_bias; - float vmax = vlen(activeObst->dvel); + float vmax = len_v2(activeObst->dvel); const float vrange = vmax*(1-m_bias); const float cs = 1.0f / (float)m_sampleRadius*vrange; @@ -837,21 +781,24 @@ void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMe { for (int x = 0; x < rad; ++x) { - const float vx = res[0] + x*cs - half; - const float vy = res[1] + y*cs - half; - if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue; - spos[nspos*2+0] = vx; - spos[nspos*2+1] = vy; + 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); + 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; } - vcpy(activeObst->nvel, res); + copy_v2_v2(activeObst->nvel, res); } } |