Real Distance<Real,TVector>::GetSquared (Real tmin, Real tmax,
const TVector& velocity0, const TVector& velocity1)
{
// The assumption is that distance f(t) is a convex function. If
// f'(tmin) >= 0, then the minimum occurs at tmin. If f'(tmax) <= 0,
// then the minimum occurs at tmax. Otherwise, f'(0) < 0 and
// f'(tmax) > 0 and the minimum occurs at some t in (tmin,tmax).
Real t0 = tmin;
Real f0 = GetSquared(t0, velocity0, velocity1);
if (f0 <= ZeroThreshold)
{
// The distance is effectively zero. The objects are initially in
// contact.
mContactTime = t0;
return (Real)0;
}
Real df0 = GetDerivativeSquared(t0, velocity0, velocity1);
if (df0 >= (Real)0)
{
// The distance is increasing on [0,tmax].
mContactTime = t0;
return f0;
}
Real t1 = tmax;
Real f1 = GetSquared(t1, velocity0, velocity1);
if (f1 <= ZeroThreshold)
{
// The distance is effectively zero.
mContactTime = t1;
return (Real)0;
}
Real df1 = GetDerivativeSquared(t1, velocity0, velocity1);
if (df1 <= (Real)0)
{
// The distance is decreasing on [0,tmax].
mContactTime = t1;
return f1;
}
// Start the process with Newton's method for computing a time when the
// distance is zero. During this process we will switch to a numerical
// minimizer if we decide that the distance cannot be zero.
int i;
for (i = 0; i < MaximumIterations; ++i)
{
// Compute the next Newton's iterate.
Real t = t0 - f0/df0;
if (t >= tmax)
{
// The convexity of the graph guarantees that when this condition
// happens, the distance is always positive. Switch to a
// numerical minimizer.
break;
}
Real f = GetSquared(t, velocity0, velocity1);
if (f <= ZeroThreshold)
{
// The distance is effectively zero.
mContactTime = t;
return (Real)0;
}
Real df = GetDerivativeSquared(t, velocity0, velocity1);
if (df >= (Real)0)
{
// The convexity of the graph guarantees that when this condition
// happens, the distance is always positive. Switch to a
// numerical minimizer.
break;
}
t0 = t;
f0 = f;
df0 = df;
}
if (i == MaximumIterations)
{
// Failed to converge within desired number of iterations. To
// reach here, the derivative values were always negative, so report
// the distance at the last time.
mContactTime = t0;
return f0;
}
// The distance is always positive. Use bisection to find the root of
// the derivative function.
Real tm = t0;
for (i = 0; i < MaximumIterations; ++i)
{
tm = ((Real)0.5)*(t0 + t1);
Real dfm = GetDerivativeSquared(tm, velocity0, velocity1);
Real product = dfm*df0;
if (product < -ZeroThreshold)
{
t1 = tm;
df1 = dfm;
}
else if (product > ZeroThreshold)
{
t0 = tm;
df0 = dfm;
}
else
{
break;
}
}
// This is the time at which the minimum occurs and is not the contact
// time. Store it anyway for debugging purposes.
mContactTime = tm;
Real fm = GetSquared(tm, velocity0, velocity1);
return fm;
}
Real SEDistance<Real, TVector>::GetSquared(Real fTMin, Real fTMax,
const TVector& rVelocity0, const TVector& rVelocity1)
{
// 假设基于函数f(t)是convex function的前提下.
// 如果f'(tmin) >= 0,则最小值在tmin处.
// 如果f'(tmax) <= 0,则最小值在tmax处.
// 否则,f'(tmin) < 0且f'(tmax) > 0,最小值在(tmin,tmax)区间内.
Real fT0 = fTMin;
Real fF0 = GetSquared(fT0, rVelocity0, rVelocity1);
if( fF0 <= ZeroThreshold )
{
// 距离几乎为零.
// 对象初始时刻接触.
m_fContactTime = fT0;
return (Real)0.0;
}
Real fDF0 = GetDerivativeSquared(fT0, rVelocity0, rVelocity1);
if( fDF0 >= (Real)0.0 )
{
// 距离在[tmin, tmax]上持续增大.
m_fContactTime = fT0;
return fF0;
}
Real fT1 = fTMax;
Real fF1 = GetSquared(fT1, rVelocity0, rVelocity1);
if( fF1 <= ZeroThreshold )
{
// 距离几乎为零.
m_fContactTime = fT1;
return (Real)0.0;
}
Real fDF1 = GetDerivativeSquared(fT1, rVelocity0, rVelocity1);
if( fDF1 <= (Real)0.0 )
{
// 距离在[tmin, tmax]上持续减小.
m_fContactTime = fT1;
return fF1;
}
// 用牛顿法计算函数值为0时的自变量t值.
// 在该过程中,如果距离不能达到0值,则切换到一个numerical minimizer.
int i;
for( i = 0; i < MaximumIterations; i++ )
{
// 计算下一次牛顿迭代位置.
Real fT = fT0 - fF0/fDF0;
if( fT >= fTMax )
{
// 函数图形的凸性确保当这种情况发生时,距离总是正值.
// 切换到一个numerical minimizer.
break;
}
Real fF = GetSquared(fT, rVelocity0, rVelocity1);
if( fF <= ZeroThreshold )
{
// 距离几乎为零.
m_fContactTime = fT;
return (Real)0.0;
}
Real fDF = GetDerivativeSquared(fT, rVelocity0, rVelocity1);
if( fDF >= (Real)0.0 )
{
// 函数图形的凸性确保当这种情况发生时,距离总是正值.
// 切换到一个numerical minimizer.
break;
}
fT0 = fT;
fF0 = fF;
fDF0 = fDF;
}
if( i == MaximumIterations )
{
// 在指定迭代次数内函数没能收敛到0值附近.
// 到达这里时,函数的导数必定都为负值,
// 因此把最后一次迭代时刻t的函数值作为距离值返回.
m_fContactTime = fT0;
return fF0;
}
// 距离总是正值.
// 用二分法查找导函数的根.
Real fTm = fT0;
for( i = 0; i < MaximumIterations; i++ )
{
fTm = ((Real)0.5)*(fT0 + fT1);
Real fDFm = GetDerivativeSquared(fTm, rVelocity0, rVelocity1);
Real fProduct = fDFm*fDF0;
if( fProduct < -ZeroThreshold )
{
fT1 = fTm;
fDF1 = fDFm;
}
else if( fProduct > ZeroThreshold )
{
fT0 = fTm;
fDF0 = fDFm;
}
else
{
break;
}
}
// 这是函数发生最小值的t值,但不是contact time.
// 存储该值用于debugging.
m_fContactTime = fTm;
Real fFm = GetSquared(fTm, rVelocity0, rVelocity1);
return fFm;
}
Real Distance<Real,TVector>::GetSquared (Real fTMin, Real fTMax,
const TVector& rkVelocity0, const TVector& rkVelocity1)
{
// The assumption is that distance f(t) is a convex function. If
// f'(tmin) >= 0, then the minimum occurs at tmin. If f'(tmax) <= 0,
// then the minimum occurs at tmax. Otherwise, f'(0) < 0 and
// f'(tmax) > 0 and the minimum occurs at some t in (tmin,tmax).
Real fT0 = fTMin;
Real fF0 = GetSquared(fT0,rkVelocity0,rkVelocity1);
if (fF0 <= ZeroThreshold)
{
// The distance is effectively zero. The objects are initially in
// contact.
m_fContactTime = fT0;
return (Real)0.0;
}
Real fDF0 = GetDerivativeSquared(fT0,rkVelocity0,rkVelocity1);
if (fDF0 >= (Real)0.0)
{
// The distance is increasing on [0,tmax].
m_fContactTime = fT0;
return fF0;
}
Real fT1 = fTMax;
Real fF1 = GetSquared(fT1,rkVelocity0,rkVelocity1);
if (fF1 <= ZeroThreshold)
{
// The distance is effectively zero.
m_fContactTime = fT1;
return (Real)0.0;
}
Real fDF1 = GetDerivativeSquared(fT1,rkVelocity0,rkVelocity1);
if (fDF1 <= (Real)0.0)
{
// The distance is decreasing on [0,tmax].
m_fContactTime = fT1;
return fF1;
}
// Start the process with Newton's method for computing a time when the
// distance is zero. During this process we will switch to a numerical
// minimizer if we decide that the distance cannot be zero.
int i;
for (i = 0; i < MaximumIterations; i++)
{
// compute the next Newton's iterate
Real fT = fT0 - fF0/fDF0;
if (fT >= fTMax)
{
// The convexity of the graph guarantees that when this condition
// happens, the distance is always positive. Switch to a
// numerical minimizer.
break;
}
Real fF = GetSquared(fT,rkVelocity0,rkVelocity1);
if (fF <= ZeroThreshold)
{
// The distance is effectively zero.
m_fContactTime = fT;
return (Real)0.0;
}
Real fDF = GetDerivativeSquared(fT,rkVelocity0,rkVelocity1);
if (fDF >= (Real)0.0)
{
// The convexity of the graph guarantees that when this condition
// happens, the distance is always positive. Switch to a
// numerical minimizer.
break;
}
fT0 = fT;
fF0 = fF;
fDF0 = fDF;
}
if (i == MaximumIterations)
{
// Failed to converge within desired number of iterations. To
// reach here, the derivative values were always negative, so report
// the distance at the last time.
m_fContactTime = fT0;
return fF0;
}
// The distance is always positive. Use bisection to find the root of
// the derivative function.
Real fTm = fT0;
for (i = 0; i < MaximumIterations; i++)
{
fTm = ((Real)0.5)*(fT0 + fT1);
Real fDFm = GetDerivativeSquared(fTm,rkVelocity0,rkVelocity1);
Real fProduct = fDFm*fDF0;
if (fProduct < -ZeroThreshold)
{
fT1 = fTm;
fDF1 = fDFm;
}
else if (fProduct > ZeroThreshold)
{
fT0 = fTm;
fDF0 = fDFm;
}
else
{
break;
}
}
// This is the time at which the minimum occurs and is not the contact
// time. Store it anyway for debugging purposes.
m_fContactTime = fTm;
Real fFm = GetSquared(fTm,rkVelocity0,rkVelocity1);
return fFm;
}
