forked from bartvdbraak/blender
KX_ObstacleSimulation: replace inline math functions with BLI_math functions
This commit is contained in:
parent
93f5e2218a
commit
c0b73fa1b0
@ -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 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 sqr(float x) { return x * x; }
|
||||||
inline float lerp(float a, float b, float t) { return a + (b-a)*t; }
|
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 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; }
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
|
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();
|
obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y();
|
||||||
|
|
||||||
// Update velocity history and calculate perceived (average) velocity.
|
// 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;
|
obs->hhead = (obs->hhead+1) % VEL_HIST_SIZE;
|
||||||
vset(obs->pvel,0,0);
|
vset(obs->pvel,0,0);
|
||||||
for (int j = 0; j < VEL_HIST_SIZE; ++j)
|
for (int j = 0; j < VEL_HIST_SIZE; ++j)
|
||||||
vadd(obs->pvel, obs->pvel, &obs->hvel[j*2]);
|
add_v2_v2v2(obs->pvel, obs->pvel, &obs->hvel[j * 2]);
|
||||||
vscale(obs->pvel, obs->pvel, 1.0f/VEL_HIST_SIZE);
|
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.
|
// Fake dynamic constraint.
|
||||||
float dv[2];
|
float dv[2];
|
||||||
float vel[2];
|
float vel[2];
|
||||||
vsub(dv, activeObst->nvel, activeObst->vel);
|
sub_v2_v2v2(dv, activeObst->nvel, activeObst->vel);
|
||||||
float ds = vlen(dv);
|
float ds = len_v2(dv);
|
||||||
if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
|
if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
|
||||||
vscale(dv, dv, fabs(maxDeltaSpeed/ds));
|
mul_v2_fl(dv, fabs(maxDeltaSpeed / ds));
|
||||||
vadd(vel, activeObst->vel, dv);
|
add_v2_v2v2(vel, activeObst->vel, dv);
|
||||||
|
|
||||||
velocity.x() = vel[0];
|
velocity.x() = vel[0];
|
||||||
velocity.y() = vel[1];
|
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)
|
if (ob->m_shape == KX_OBSTACLE_CIRCLE)
|
||||||
{
|
{
|
||||||
MT_Vector2 vab;
|
MT_Vector2 vab;
|
||||||
if (vlen(ob->vel) < 0.01f*0.01f)
|
if (len_v2(ob->vel) < 0.01f * 0.01f) {
|
||||||
{
|
|
||||||
// Stationary, use VO
|
// Stationary, use VO
|
||||||
vab = svel;
|
vab = svel;
|
||||||
}
|
}
|
||||||
@ -591,8 +540,7 @@ void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMes
|
|||||||
tc.toie[iter] = tmine;
|
tc.toie[iter] = tmine;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (vlen(activeObst->vel) > 0.1)
|
if (len_v2(activeObst->vel) > 0.1f) {
|
||||||
{
|
|
||||||
// Constrain max turn rate.
|
// Constrain max turn rate.
|
||||||
float cura = atan2(activeObst->vel[1],activeObst->vel[0]);
|
float cura = atan2(activeObst->vel[1],activeObst->vel[0]);
|
||||||
float da = bestDir - cura;
|
float da = bestDir - cura;
|
||||||
@ -632,11 +580,10 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM
|
|||||||
const float ivmax = 1.0f / vmax;
|
const float ivmax = 1.0f / vmax;
|
||||||
|
|
||||||
float adir[2] /*, adist */;
|
float adir[2] /*, adist */;
|
||||||
vcpy(adir, activeObst->pvel);
|
if (normalize_v2_v2(adir, activeObst->pvel) <= 0.01f) {
|
||||||
if (vlen(adir) > 0.01f)
|
zero_v2(adir);
|
||||||
vnorm(adir);
|
}
|
||||||
else
|
|
||||||
vset(adir,0,0);
|
|
||||||
float activeObstPos[2];
|
float activeObstPos[2];
|
||||||
vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y());
|
vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y());
|
||||||
/* adist = vdot(adir, activeObstPos); */
|
/* adist = vdot(adir, activeObstPos); */
|
||||||
@ -646,7 +593,7 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM
|
|||||||
for (int n = 0; n < nspos; ++n)
|
for (int n = 0; n < nspos; ++n)
|
||||||
{
|
{
|
||||||
float vcand[2];
|
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.
|
// Find min time of impact and exit amongst all obstacles.
|
||||||
float tmin = maxToi;
|
float tmin = maxToi;
|
||||||
@ -666,9 +613,9 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM
|
|||||||
float vab[2];
|
float vab[2];
|
||||||
|
|
||||||
// Moving, use RVO
|
// Moving, use RVO
|
||||||
vscale(vab, vcand, 2);
|
mul_v2_v2fl(vab, vcand, 2);
|
||||||
vsub(vab, vab, activeObst->vel);
|
sub_v2_v2v2(vab, vab, activeObst->vel);
|
||||||
vsub(vab, vab, ob->vel);
|
sub_v2_v2v2(vab, vab, ob->vel);
|
||||||
|
|
||||||
// Side
|
// Side
|
||||||
// NOTE: dp, and dv are constant over the whole calculation,
|
// 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];
|
float pb[2];
|
||||||
vset(pb, ob->m_pos.x(), ob->m_pos.y());
|
vset(pb, ob->m_pos.x(), ob->m_pos.y());
|
||||||
|
|
||||||
const float orig[2] = {0,0};
|
const float orig[2] = {0, 0};
|
||||||
float dp[2],dv[2],np[2];
|
float dp[2], dv[2], np[2];
|
||||||
vsub(dp,pb,pa);
|
sub_v2_v2v2(dp, pb, pa);
|
||||||
vnorm(dp);
|
normalize_v2(dp);
|
||||||
vsub(dv,ob->dvel, activeObst->dvel);
|
sub_v2_v2v2(dv, ob->dvel, activeObst->dvel);
|
||||||
|
|
||||||
const float a = triarea(orig, dp,dv);
|
/* TODO: use line_point_side_v2 */
|
||||||
if (a < 0.01f)
|
if (area_tri_signed_v2(orig, dp, dv) < 0.01f) {
|
||||||
{
|
|
||||||
np[0] = -dp[1];
|
np[0] = -dp[1];
|
||||||
np[1] = dp[0];
|
np[1] = dp[0];
|
||||||
}
|
}
|
||||||
else
|
else {
|
||||||
{
|
|
||||||
np[0] = dp[1];
|
np[0] = dp[1];
|
||||||
np[1] = -dp[0];
|
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++;
|
nside++;
|
||||||
|
|
||||||
if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad,
|
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.
|
// 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!
|
// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
|
||||||
const float r = 0.01f; // agent->rad
|
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];
|
float sdir[2], snorm[2];
|
||||||
vsub(sdir, q, p);
|
sub_v2_v2v2(sdir, q, p);
|
||||||
snorm[0] = sdir[1];
|
snorm[0] = sdir[1];
|
||||||
snorm[1] = -sdir[0];
|
snorm[1] = -sdir[0];
|
||||||
// If the velocity is pointing towards the segment, no collision.
|
// If the velocity is pointing towards the segment, no collision.
|
||||||
if (vdot(snorm, vcand) < 0.0f)
|
if (dot_v2v2(snorm, vcand) < 0.0f)
|
||||||
continue;
|
continue;
|
||||||
// Else immediate collision.
|
// Else immediate collision.
|
||||||
htmin = 0.0f;
|
htmin = 0.0f;
|
||||||
@ -767,17 +712,16 @@ static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavM
|
|||||||
if (nside)
|
if (nside)
|
||||||
side /= nside;
|
side /= nside;
|
||||||
|
|
||||||
const float vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
|
const float vpen = velWeight * (len_v2v2(vcand, activeObst->dvel) * ivmax);
|
||||||
const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
|
const float vcpen = curVelWeight * (len_v2v2(vcand, activeObst->vel) * ivmax);
|
||||||
const float spen = sideWeight * side;
|
const float spen = sideWeight * side;
|
||||||
const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));
|
const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));
|
||||||
|
|
||||||
const float penalty = vpen + vcpen + spen + tpen;
|
const float penalty = vpen + vcpen + spen + tpen;
|
||||||
|
|
||||||
if (penalty < minPenalty)
|
if (penalty < minPenalty) {
|
||||||
{
|
|
||||||
minPenalty = penalty;
|
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)
|
const float maxDeltaAngle)
|
||||||
{
|
{
|
||||||
vset(activeObst->nvel, 0.f, 0.f);
|
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];
|
float* spos = new float[2*m_maxSamples];
|
||||||
int nspos = 0;
|
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 cvx = activeObst->dvel[0]*m_bias;
|
||||||
const float cvy = activeObst->dvel[1]*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 vrange = vmax*(1-m_bias);
|
||||||
const float cs = 1.0f / (float)m_sampleRadius*vrange;
|
const float cs = 1.0f / (float)m_sampleRadius*vrange;
|
||||||
|
|
||||||
@ -837,11 +781,14 @@ void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMe
|
|||||||
{
|
{
|
||||||
for (int x = 0; x < rad; ++x)
|
for (int x = 0; x < rad; ++x)
|
||||||
{
|
{
|
||||||
const float vx = res[0] + x*cs - half;
|
const float v_xy[2] = {
|
||||||
const float vy = res[1] + y*cs - half;
|
res[0] + x * cs - half,
|
||||||
if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
|
res[1] + y * cs - half};
|
||||||
spos[nspos*2+0] = vx;
|
|
||||||
spos[nspos*2+1] = vy;
|
if (len_squared_v2(v_xy) > sqr(vmax + cs / 2))
|
||||||
|
continue;
|
||||||
|
|
||||||
|
copy_v2_v2(&spos[nspos * 2 + 0], v_xy);
|
||||||
nspos++;
|
nspos++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -851,7 +798,7 @@ void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMe
|
|||||||
|
|
||||||
cs *= 0.5f;
|
cs *= 0.5f;
|
||||||
}
|
}
|
||||||
vcpy(activeObst->nvel, res);
|
copy_v2_v2(activeObst->nvel, res);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user