Real SEDistance<Real, TVector>::GetDerivativeSquared(Real fT, const TVector& rVelocity0, const TVector& rVelocity1) { // 派生类如果有更快的方法计算距离平方的导数,则应重载这个函数. Real fDistance = Get(fT, rVelocity0, rVelocity1); Real fDerivative = GetDerivative(fT, rVelocity0, rVelocity1); return ((Real)2.0)*fDistance*fDerivative; }
Real Distance<Real,TVector>::GetDerivativeSquared (Real t, const TVector& velocity0, const TVector& velocity1) { // A derived class should override this only if there is a faster method // to compute the derivative of the squared distance for the specific // class. Real distance = Get(t, velocity0, velocity1); Real derivative = GetDerivative(t, velocity0, velocity1); return ((Real)2)*distance*derivative; }
Real SEDistance<Real, TVector>::Get(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 = Get(fT0, rVelocity0, rVelocity1); if( fF0 <= ZeroThreshold ) { // 距离几乎为零. // 对象初始时刻接触. m_fContactTime = fT0; return (Real)0.0; } Real fDF0 = GetDerivative(fT0, rVelocity0, rVelocity1); if( fDF0 >= (Real)0.0 ) { // 距离在[tmin, tmax]上持续增大. m_fContactTime = fT0; return fF0; } Real fT1 = fTMax; Real fF1 = Get(fT1, rVelocity0, rVelocity1); if( fF1 <= ZeroThreshold ) { // 距离几乎为零. m_fContactTime = fT1; return (Real)0.0; } Real fDF1 = GetDerivative(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 = Get(fT, rVelocity0, rVelocity1); if( fF <= ZeroThreshold ) { // 距离几乎为零. m_fContactTime = fT; return (Real)0.0; } Real fDF = GetDerivative(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 = GetDerivative(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 = Get(fTm, rVelocity0, rVelocity1); return fFm; }
Real Distance<Real,TVector>::Get (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 = Get(t0, velocity0, velocity1); if (f0 <= ZeroThreshold) { // The distance is effectively zero. The objects are initially in // contact. mContactTime = t0; return (Real)0; } Real df0 = GetDerivative(t0, velocity0, velocity1); if (df0 >= (Real)0) { // The distance is increasing on [0,tmax]. mContactTime = t0; return f0; } Real t1 = tmax; Real f1 = Get(t1, velocity0, velocity1); if (f1 <= ZeroThreshold) { // The distance is effectively zero. mContactTime = t1; return (Real)0; } Real df1 = GetDerivative(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 = Get(t, velocity0, velocity1); if (f <= ZeroThreshold) { // The distance is effectively zero. mContactTime = t; return (Real)0; } Real df = GetDerivative(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 = GetDerivative(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 = Get(tm, velocity0, velocity1); return fm; }
Real Distance<Real,TVector>::Get (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 = Get(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 = GetDerivative(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 = Get(fT1,rkVelocity0,rkVelocity1); if (fF1 <= ZeroThreshold) { // The distance is effectively zero. m_fContactTime = fT1; return (Real)0.0; } Real fDF1 = GetDerivative(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 = Get(fT,rkVelocity0,rkVelocity1); if (fF <= ZeroThreshold) { // The distance is effectively zero. m_fContactTime = fT; return (Real)0.0; } Real fDF = GetDerivative(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 = GetDerivative(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 = Get(fTm,rkVelocity0,rkVelocity1); return fFm; }