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