Real LodCollapseCostQuadric::computeEdgeCollapseCost( LodData* data, LodData::Vertex* src, LodData::Edge* dstEdge )
{
LodData::Vertex* dst = dstEdge->dst;
if (isBorderVertex(src)) {
return LodData::NEVER_COLLAPSE_COST;
}
if (src->seam && !dst->seam) {
return LodData::NEVER_COLLAPSE_COST;
}
Matrix4 Qnew = mVertexQuadricList[LodData::getVectorIDFromPointer(data->mVertexList, src)] +
mVertexQuadricList[LodData::getVectorIDFromPointer(data->mVertexList, dst)];
Vector4 Vnew(dst->position);
// error = Vnew^T * Qnew * Vnew
Real cost =
(Vnew[0] * Qnew[0][0] + Vnew[1] * Qnew[0][1] + Vnew[2] * Qnew[0][2] + Vnew[3] * Qnew[0][3]) * Vnew[0] +
(Vnew[0] * Qnew[1][0] + Vnew[1] * Qnew[1][1] + Vnew[2] * Qnew[1][2] + Vnew[3] * Qnew[1][3]) * Vnew[1] +
(Vnew[0] * Qnew[2][0] + Vnew[1] * Qnew[2][1] + Vnew[2] * Qnew[2][2] + Vnew[3] * Qnew[2][3]) * Vnew[2] +
(Vnew[0] * Qnew[3][0] + Vnew[1] * Qnew[3][1] + Vnew[2] * Qnew[3][2] + Vnew[3] * Qnew[3][3]) * Vnew[3];
if (dst->seam) {
cost *= 8;
}
return cost;
}
double ppm(
const Matrix<T,N,1> & Delta,
const Matrix<T,N,1> & Vmax,
const Matrix<T,N,1> & Amax,
Matrix<T,N,1> & Vnew,
Matrix<T,N,1> & Anew,
Matrix<T,N,1> & Dnew,
T requestedMotionTime = 0
)
{
Matrix<T,N,3> Times;
// Iterate over axes
for(unsigned int l = 0; l < N; ++l) {
const T delta = Delta(l,0), // motor position delta
vmax = Vmax(l,0), // maximal velocity
amax = Amax(l,0), // maximal acceleration
dmax = -Amax(l,0); // maximal deceleration
// Velocity value, when the velocity profile is triangular (eq. 3.32)
const T VTriangle = amax*std::sqrt(2*delta*dmax/(amax*(dmax-amax)));
//const T VTriangle = std::sqrt(2*delta*amax*dmax/(dmax-amax));
#if 0
std::cout << "VTriangle(" << l << ") = " << VTriangle << std::endl;
#endif
const bool TriangularProfile = (VTriangle <= vmax);
if(TriangularProfile) {
// tt: total motion time (eq. 3.33)
Times(l,2) = std::sqrt(2*delta*(dmax-amax)/(amax*dmax));
// acceleration and deceleration phase treated as a half of the total motion time
Times(l,0) = Times(l,1) = Times(l,2)/2;
} else {
// ta: time to stop accelerate (eq. 3.35)
Times(l,0) = vmax/amax;
// td: time to start deceleration (eq. 3.42)
Times(l,1) = delta/vmax + vmax/(2*amax) + vmax/(2*dmax);
// Numerically stable version:
//Time(l,1) = (2*delta*amax*dmax+vmax*vmax*amax+vmax*vmax*dmax)/(2*vmax*amax*dmax);
// tt: total motion time (eq. 3.40)
Times(l,2) = delta/vmax + vmax/(2*amax) - vmax/(2*dmax);
// Numerically stable version:
//Time(l,2) = (2*delta*amax*dmax+vmax*vmax*dmax-vmax*vmax*amax)/(2*vmax*amax*dmax);
}
// std::cerr << "VLimit[" << l << "]: " << VTriangle <<
// " => " << (TriangularProfile ? "triangular" : "trapezoidal") << std::endl <<
// "Time[" << l << "]: " << Times(l,0) << " " << Times(l,1) << " " << Times(l,2) <<
// std::endl;
}
Matrix<T,1,3> maxTimes = Times.colwise().maxCoeff();
// std::cerr << "maxTimes: " << maxTimes << std::endl;
// max of acceleration intervals
const T ta = maxTimes(0);
// max of constant velocity intervals
const T tV = (Times.col(1)-Times.col(0)).maxCoeff();
// max of deceleration intervals
const T tD = (Times.col(2)-Times.col(1)).maxCoeff();
// deceleration interval
const T td = ta + tV;
T tt = ta + tV + tD;
if (ta > td) {
tt += (ta - td);
}
// Make the motion longer
if(requestedMotionTime > 0) {
if(tt > requestedMotionTime) {
throw std::runtime_error("requested motion time too short");
}
tt = requestedMotionTime;
}
// std::cout
// << "ta: " << ta << "\t"
// << "td: " << td << "\t"
// << "tt: " << tt
// << std::endl;
// If the total time is zero there will be no motion
if(tt > 0) {
// I guess this can be implemented as a single matrix calculation
for(unsigned int l = 0; l < N; ++l) {
const T delta = Delta(l,0);
// Calculate new parameters if there is motion along an axis
if(delta) {
Anew(l,0) = 2*delta/(ta*(tt+td-ta));
Dnew(l,0) = - (-2*delta/((tt+td-ta)*(tt-td))); // deceleration value (without sign)
//Dnew(l,0) = (2*delta)/(tt*tt-tt*ta+td*ta-td*td); // Numerically stable (?) version
Vnew(l,0) = Anew(l,0)*ta;
//Vnew(l,0) = (2*delta)/(tt+td-ta); // Numerically stable (?) version
} else {
Anew(l,0) = Amax(l,0);
Dnew(l,0) = Amax(l,0);
Vnew(l,0) = Vmax(l,0);
}
}
} else {
Vnew = Vmax;
Anew = Dnew = Amax;
}
// These assertions fail because of floating point inequalities
//assert(Dnew(l,0)<=Amax(l,0));
//assert(Anew(l,0)<=Amax(l,0));
//assert(Vnew(l,0)<=Vmax(l,0));;
// std::cerr <<
// "Vnew:\n" << Vnew << std::endl <<
// "Anew:\n" << Anew << std::endl <<
// "Dnew:\n" << Dnew << std::endl <<
// std::endl;
// std::cerr << "tt: " << tt << std::endl;
return tt;
}
