State * takeastep(State * previous_state) { // calculates the next state in the simulation
// this is where the node::update funcs are called
State * state = new State;
float deltas;
node * ptr = previous_state->root->right;
MatrixXf M = MatrixXf::Zero(3*(NUM-1)+1, 3*(NUM-1)+1); //genralized mass matrix for the entire system
for(int i=0; i<3*(NUM-1); i+=3) {
// diagonal entryies = 2*deltas, off diagonal = deltas
deltas = ptr->right->material_coordinate - ptr->material_coordinate;
M(i,i) += 2*deltas;
M(i,i+1) += 2*deltas;
M(i,i+2) += 2*deltas;
M(i+1,i) += 2*deltas;
M(i+2,i) += 2*deltas;
M(i+1,i+1) += 2*deltas;
M(i+1,i+2) += 2*deltas;
M(i+1,i+2) += 2*deltas;
M(i+2,i+2) += 2*deltas;
if(i == 3*(NUM-2))
break;
M(i+3, i) += deltas;
M(i+3, i+1) += deltas;
M(i+3, i+2) += deltas;
M(i+4, i) += deltas;
M(i+5, i) += deltas;
M(i+4, i+1) += deltas;
M(i+4, i+1) += deltas;
M(i+4, i+1) += deltas;
M(i+5, i+2) += deltas;
M(i, i+3) = deltas;
M(i+1, i+3) = deltas;
M(i+2, i+3) = deltas;
M(i, i+4) = deltas;
M(i, i+5) = deltas;
M(i+1, i+4) = deltas;
M(i+1, i+4) = deltas;
M(i+1, i+4) = deltas;
M(i+2, i+5) = deltas;
M(i+3, i+3) += 2*deltas;
M(i+3, i+4) += 2*deltas;
M(i+3, i+5) += 2*deltas;
M(i+4, i+3) += 2*deltas;
M(i+5, i+3) += 2*deltas;
M(i+4, i+4) += 2*deltas;
M(i+4, i+5) += 2*deltas;
M(i+5, i+4) += 2*deltas;
M(i+5, i+5) += 2*deltas;
ptr = ptr->right;
i+=3;
}
node * right_of_e0 = linear_search(previous_state->e1->material_coordinate, previous_state->root);
float alpha = (previous_state->e1->material_coordinate - right_of_e0->left->material_coordinate)/(right_of_e0->material_coordinate - right_of_e0->left->material_coordinate);
Vector3f deltax = *(right_of_e0->world_x)-*(right_of_e0->left->world_x);
float mss = deltax.dot(deltax)/(right_of_e0->material_coordinate - right_of_e0->left->material_coordinate);
M(3*(NUM-1), 3*(NUM-1)) = 2*mss;
M(3*(NUM-1)-3, 3*(NUM-1)) += -deltax.x();
M(3*(NUM-1)-2, 3*(NUM-1)) += -deltax.y();
M(3*(NUM-1)-1, 3*(NUM-1)) += -deltax.z();
M(3*(NUM-1), 3*(NUM-1)-3) += -deltax.x();
M(3*(NUM-1), 3*(NUM-1)-2) += -deltax.y();
M(3*(NUM-1), 3*(NUM-1)-1) += -deltax.z();
// force term : taking into account of gravity and spring potential energy
VectorXf f = VectorXf(3*(NUM-1)+1);
VectorXf qOldDot = VectorXf(3*(NUM-1)+1);
VectorXf qOld = VectorXf(3*(NUM-1)+1);
ptr = previous_state->root->right;
Vector3f deltaXDotRight, deltaXDotLeft, deltaXRight, deltaXLeft, constant;
float absDeltaXRight, absDeltaXLeft, restLengthRight, restLengthLeft;
for(int i=0; i<3*(NUM-1); i+=3 ) {
//putting in old values for Euler integration
qOldDot[i] = ptr->world_x_dot->x();
qOldDot[i+1] = ptr->world_x_dot->y();
qOldDot[i+2] = ptr->world_x_dot->z();
qOld[i] = ptr->world_x->x();
qOld[i+1] = ptr->world_x->y();
qOld[i+2] = ptr->world_x->z();
//gravity
f[i+1] = -g * ptr->rho * (ptr->material_coordinate - ptr->left->material_coordinate);
//spring
if(ptr != right_of_e0->left && ptr != right_of_e0) { //normal case
deltaXDotRight = *(ptr->world_x_dot) - *(ptr->right->world_x_dot);
deltaXDotLeft = *(ptr->world_x_dot) - *(ptr->left->world_x_dot);
deltaXRight = *(ptr->world_x) - *(ptr->right->world_x);
deltaXLeft = *(ptr->world_x) - *(ptr->left->world_x);
absDeltaXRight = sqrt(deltaXRight.dot(deltaXRight));
absDeltaXLeft = sqrt(deltaXLeft.dot(deltaXLeft));
constant = -(ks * (absDeltaXLeft - l0) + kd * deltaXDotLeft.dot(deltaXLeft)/absDeltaXLeft)*deltaXLeft/absDeltaXLeft - (ks * (absDeltaXRight - l0) + kd * deltaXDotRight.dot(deltaXRight)/absDeltaXRight)*deltaXRight/absDeltaXRight;
}
else if(ptr == right_of_e0->left) { //ball's left node is ptr
deltaXDotRight = *(ptr->world_x_dot) - *(previous_state->e1->world_x_dot);
deltaXDotLeft = *(ptr->world_x_dot) - *(ptr->left->world_x_dot);
deltaXRight = *(ptr->world_x) - *(previous_state->e1->world_x);
deltaXLeft = *(ptr->world_x) - *(ptr->left->world_x);
restLengthRight = l0 * alpha;
restLengthRight = l0;
if(deltaXRight.dot(deltaXRight) < 0.001) {
deltaXDotRight = *(ptr->world_x_dot) - *(ptr->right->world_x_dot);
deltaXRight = *(ptr->world_x) - *(ptr->right->world_x);
restLengthLeft = l0;
}
absDeltaXRight = sqrt(deltaXRight.dot(deltaXRight));
absDeltaXLeft = sqrt(deltaXLeft.dot(deltaXLeft));
constant = -(ks * (absDeltaXLeft - restLengthLeft) + kd * deltaXDotLeft.dot(deltaXLeft)/absDeltaXLeft)*deltaXLeft/absDeltaXLeft - (ks * (absDeltaXRight - restLengthRight) + kd * deltaXDotRight.dot(deltaXRight)/absDeltaXRight)*deltaXRight/absDeltaXRight;
}
else if(ptr == right_of_e0) { //ball's right node is ptr
deltaXDotLeft = *(ptr->world_x_dot) - *(previous_state->e1->world_x_dot);
deltaXDotRight = *(ptr->world_x_dot) - *(ptr->right->world_x_dot);
deltaXLeft = *(ptr->world_x) - *(previous_state->e1->world_x);
deltaXRight = *(ptr->world_x) - *(ptr->right->world_x);
restLengthLeft = l0 * alpha;
restLengthRight = l0;
if(deltaXLeft.dot(deltaXLeft) < 0.001) {
deltaXDotLeft = *(ptr->world_x_dot) - *(ptr->left->world_x_dot);
deltaXLeft = *(ptr->world_x) - *(ptr->left->world_x);
restLengthLeft = l0;
}
absDeltaXLeft = sqrt(deltaXLeft.dot(deltaXLeft));
absDeltaXRight = sqrt(deltaXRight.dot(deltaXRight));
constant = -(ks * (absDeltaXLeft - restLengthLeft) + kd * deltaXDotLeft.dot(deltaXLeft)/absDeltaXLeft)*deltaXLeft/absDeltaXLeft - (ks * (absDeltaXRight - restLengthRight) + kd * deltaXDotRight.dot(deltaXRight)/absDeltaXRight)*deltaXRight/absDeltaXRight;
}
f[i] += constant.x();
f[i+1] += constant.y();
f[i+2] += constant.z();
f[i] = 0.0; f[i+2] = 0.0;
ptr = ptr->right;
}
Vector3f x0_and_x1= (1-alpha)* *(right_of_e0->left->world_x) + alpha* *(right_of_e0->world_x);
f[3*(NUM-1)] = previous_state->e1->rho * x0_and_x1.dot(Vector3f(0,-g,0));
VectorXf rhs = M * qOldDot + dt * f;
VectorXf qNewDot;
qNewDot = M.lu().solve(rhs);
VectorXf qNew = qNewDot * dt + qOld;
L3node * newRoot = new L3node(0.0, *(previous_state->root->world_x), *(previous_state->root->world_x_dot));
state->root = newRoot;
node * newPtr = state->root;
ptr = previous_state->root->right;
Vector3f newX, newXDot;
//update the new state
for(int i=0; i<3*(NUM-1); i+=3) {
newX = Vector3f(qNew[i], qNew[i+1], qNew[i+2]);
newXDot = Vector3f(qNewDot[i], qNewDot[i+1], qNewDot[i+2]);
L3node * newNode = new L3node(ptr->material_coordinate, newX, newXDot);
newPtr->right = newNode;
newPtr->right->left = newPtr;
ptr = ptr->right;
newPtr = newPtr->right;
}
L3node* rightNode = new L3node(1.0, *(ptr->world_x), *(ptr->world_x_dot));
newPtr->right = rightNode;
newPtr->right->left = newPtr;
cout << "qNew : " << qNew << "qDotNew: " << qNewDot << endl;
right_of_e0 = linear_search(qNew[3*(NUM-1)], newRoot);
alpha = (qNew[3*(NUM-1)] - right_of_e0->left->material_coordinate) / (right_of_e0->material_coordinate - right_of_e0->left->material_coordinate);
newX = (1.0-alpha) * *(right_of_e0->left->world_x) + alpha * *(right_of_e0->world_x);
newXDot = (1.0-alpha) * *(right_of_e0->left->world_x_dot) + alpha * *(right_of_e0->world_x_dot);
E0node * newE = new E0node(qNew[3*(NUM-1)], qNewDot[3*(NUM-1)], newX, newXDot, newRoot);
newE->rho = 2.0;
state->e1 = newE;
state->next = NULL;
return state;
/*
// state->e1 = dynamic_cast<E3node *> (previous_state->e1)->update();
//state->e1 = dynamic_cast<L3node *> (previous_state->e1)->update();
state->e1 = dynamic_cast<E0node *> (previous_state->e1)->update();
state->root = dynamic_cast<E0node *> (state->e1)->rootNode;
state->next = NULL;
return state;
*/
}
