int main() {
int v1, v2;
scanf("%d %d", &v1, &v2);
int t;
scanf("%d %d", &t, &d);
int result = findMaxDistance(v1, v2, t);
printf("%d\n", result);
return 0;
}
int findMaxDistance(int v1, int v2, int t) {
int v_1 = mymin(v1, v2);
int v_2 = mymax(v1, v2);
if (t == 2) {
assert(v_2 - v_1 <= d);
return v_1 + v_2;
} else {
return v_1 + findMaxDistance(v_1 + d, v_2, t - 1);
}
}
void Thread::minimize_energy()
{
//Set up optimization parameters
setStationaryOptParams();
/*
std::cout << "start: " << std::endl;
std::cout << thread_curvature_error_penalty << std::endl;
std::cout << thread_torsion_error_penalty << std::endl;
std::cout << thread_diff_curvature_error_penalty << std::endl;
std::cout << thread_diff_torsion_error_penalty << std::endl;
std::cout << position_error_penalty << std::endl;
std::cout << rotation_error_penalty << std::endl;
std::cout << total_error_penalty << std::endl;
std::cout << gravity_penalty << std::endl;
*/
ThreadPiece* maxDistPiece;
double maxDist = findMaxDistance(&maxDistPiece);
while (maxDist > DISTANCE_THRESH)
{
//decide how many pieces each piece must be split into
int numPiecesLeft, numPiecesRight;
if (maxDistPiece->_length == maxDistPiece->_next_segment->_length)
{
numPiecesLeft = 2;
numPiecesRight = 2;
} else if (maxDistPiece->_length > maxDistPiece->_next_segment->_length) {
numPiecesLeft = (int)(maxDistPiece->_length / maxDistPiece->_next_segment->_length);
numPiecesRight = 1;
} else if (maxDistPiece->_length < maxDistPiece->_next_segment->_length) {
numPiecesLeft = 1;
numPiecesRight = (int)(maxDistPiece->_next_segment->_length / maxDistPiece->_length);
}
opt_params.length_per_segment = (maxDistPiece->_length + maxDistPiece->_next_segment->_length)/((double)(numPiecesLeft+numPiecesRight));
opt_params.num_segments = numPiecesLeft+numPiecesRight;
//std::cout << "splitting into " << opt_params.num_segments << " length per segment: " << opt_params.length_per_segment << std::endl;
// opt_params.transform_back = Matrix4d::Identity();
opt_params.transform_back = _transform_to_start;
opt_params.length_back = 0.0;
opt_params.transform_front = Matrix4d::Identity();
opt_params.length_front = 0.0;
//opt_params.twist_total_other_segments = 0.0;
ThreadPiece* currPiece = threadList;
while (currPiece != maxDistPiece)
{
opt_params.transform_back *= currPiece->_transform;
opt_params.length_back += currPiece->_length;
//opt_params.twist_total_other_segments += currPiece->_length* currPiece->_torsion;
opt_params.curvature_back = currPiece->_curvature;
opt_params.torsion_back = currPiece->_torsion;
currPiece = currPiece->_next_segment;
}
currPiece = maxDistPiece->_next_segment->_next_segment;
if (currPiece != NULL)
{
opt_params.curvature_front = currPiece->_curvature;
opt_params.torsion_front = currPiece->_torsion;
}
while (currPiece != NULL)
{
opt_params.transform_front *= currPiece->_transform;
opt_params.length_front += currPiece->_length;
//opt_params.twist_total_other_segments += currPiece->_length* currPiece->_torsion;
currPiece = currPiece->_next_segment;
}
//std::cout << "twist other seg: " << opt_params.twist_total_other_segments << std::endl;
//std::cout << "transform start: " << opt_params.transform_back << std::endl;
//std::cout << "transform end: " << opt_params.transform_front << std::endl;
//set initial parameters
if (opt_params.orig_params_each_piece->nrows() < 2*opt_params.num_segments+1)
{
delete(opt_params.orig_params_each_piece);
opt_params.orig_params_each_piece = new NEWMAT::ColumnVector(2*opt_params.num_segments+1);
}
int ind = 0;
for (; ind < numPiecesLeft; ind++)
{
opt_params.orig_params_each_piece->element(2*ind) = maxDistPiece->_curvature;
opt_params.orig_params_each_piece->element(2*ind+1) = maxDistPiece->_torsion;
}
for (; ind < numPiecesLeft + numPiecesRight; ind++)
{
opt_params.orig_params_each_piece->element(2*ind) = maxDistPiece->_next_segment->_curvature;
opt_params.orig_params_each_piece->element(2*ind+1) = maxDistPiece->_next_segment->_torsion;
}
opt_params.orig_params_each_piece->element(2*opt_params.num_segments) = _angle_first_rot;
//setup OPT++ problem
NEWMAT::ColumnVector x_sol;
optimize_FDNLF(2*opt_params.num_segments+1, energyEvalFunction, energyEvalFunction_init, x_sol, 1.e-6);
//split into pieces with new parameters
double curvature[numPiecesLeft+numPiecesRight];
double torsion[numPiecesLeft+numPiecesRight];
for (int ind=0; ind < opt_params.num_segments; ind++)
{
curvature[ind] = x_sol(2*ind +1);
torsion[ind] = x_sol(2*ind+2);
}
_angle_first_rot = x_sol(opt_params.num_segments*2+1);
// std::cout << curvature[0] << " " << curvature[1] << " " << curvature[2] << " " << curvature[3] << " " << std::endl;
maxDistPiece->_next_segment->splitIntoSegments(&curvature[numPiecesLeft], &torsion[numPiecesLeft], numPiecesRight);
maxDistPiece->splitIntoSegments(curvature, torsion, numPiecesLeft);
//get next piece to be split
maxDist = findMaxDistance(&maxDistPiece);
//std::cout << "maxDist: " << maxDist << std::endl;
}
//printThreadInfo();
}
