void SystemChain::update(double theTime) { std::stringstream barName; // compute new length. Temp store double updateLength = interpolate1D(theTime, timeLenghts); cout << "currentLength = " << currentLength << ", updateLength = " << updateLength << endl; if (updateLength != currentLength) { if (updateLength > length || updateLength < 0) { cerr << "ERROR: Trying to extend CHAIN beyond its lenght!!!" << endl; } else { // Commented are possible optimizations (opt) // if( updateLength > currentLength ){ // EXTENSION for opt // Locate bar. Compute index with currentLength //int firstConstraint = floor(currentLength * segments / length); int lastConstraint = ceil(updateLength * segments / length); for (int i = 0; i < lastConstraint; ++i) { // for opt: Should say i=firstConstraint barName.str(std::string()); barName << this->title << i; if (i < lastConstraint - 1) { // Extend constraint to maximum this->localConstraintDistance[barName.str()] ->setLenght(length / segments); } else { // Must be extended precisely this->localConstraintDistance[barName.str()] ->setLenght(updateLength - i * length / segments - 1E-8); } } // } // if( updateLength < currentLength ){ // COMPRESION // // Locate bar. Compute index with currentLength // int firstConstraint = ceil( currentLength*segments/length ); // int lastConstraint = floor( updateLength*segments/length ); // for (int i=firstConstraint; i > lastConstraint; --i){ // barName.str(std::string()); // barName << this->title << i; // if(i>lastConstraint+1){ // Compress constraint to minimum // this->localConstraintDistance[barName.str()]->setLenght(0.); // } // else{ // Must be extended precisely // this->localConstraintDistance[barName.str()] // ->setLenght(updateLength-lastConstraint*length/segments); // } // } // } // Update currentLength currentLength = updateLength; } } }
float interpolateCubic1D (const float table[][2], int size, float p) { if (size < 3) return interpolate1D (table, size, p); if (p < table[0][0]) return table[0][1]; if (p >= table[size-1][0]) return table[size-1][1]; size_t i = 0; size_t j = size; while (i < j - 1) { size_t k = (i + j) / 2; if (table[k][0] == p) return table[k][1]; else if (table[k][0] < p) i = k; else j = k; } float dx = (table[i+1][0] - table[i][0]); float dy = (table[i+1][1] - table[i][1]); float m0, m1; if (i > 0) { m0 = 0.5f * (dy + dx * (table[i][1] - table[i-1][1]) / (table[i][0] - table[i-1][0])); } if (i < size - 2) { m1 = 0.5f * (dy + dx * (table[i+2][1] - table[i+1][1]) / (table[i+2][0] - table[i+1][0])); } if (i <= 0) { m0 = (3 * dy - m1) * 0.5f; } if (i >= size - 2) { m1 = (3 * dy - m0) * 0.5f; } float t = (p - table[i][0]) / dx; float t2 = t * t; float t3 = t2 * t; return table[i][1] * (2 * t3 - 3 * t2 + 1) + m0 * (t3 - 2 * t2 + t) + table[i+1][1] * (-2 * t3 + 3 * t2) + m1 * (t3 - t2); }