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; }