00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "psim.h"
00015 #include <limits>
00016
00017 #define USE_INITIAL_GUESS
00018
00019 #include <sdcontact.h>
00020 #include <lcp.h>
00021
00022
00023 #ifdef VERBOSE
00024 #include <fstream>
00025 extern std::ofstream update_log;
00026 #endif
00027
00028 int pSim::Update(double timestep, std::vector<SDContactPair*>& sdContactPairs)
00029 {
00030 ClearExtForce();
00031 int n_pairs = sdContactPairs.size();
00032 if(n_pairs == 0)
00033 {
00034 return pSim::Update();
00035 }
00036 #ifdef VERBOSE
00037 update_log << "Update(" << timestep << ")" << endl;
00038 #endif
00039 int n_total_points = 0;
00040 #ifdef VERBOSE
00041 update_log << "num pairs = " << n_pairs << endl;
00042 #endif
00043 in_create_chain = true;
00044 clear_contact();
00045
00046 for(int i=0; i<n_pairs; i++)
00047 {
00048 SDContactPair* sd_pair = sdContactPairs[i];
00049 Joint* pjoint = sd_pair->GetJoint(0);
00050 Joint* rjoint = sd_pair->GetJoint(1);
00051 int n_points = sd_pair->NumPoints();
00052 if(n_points == 0) continue;
00053 #ifdef VERBOSE
00054 update_log << "== [" << pjoint->name << ", " << rjoint->name << "]: contact points = " << n_points << endl;
00055 #endif
00056
00057 static fVec3 rjoint_lin_vel, rjoint_ang_vel, rjoint_pos, temp;
00058 static fMat33 t_p_att, rjoint_att, t_rjoint_att;
00059 t_p_att.tran(pjoint->abs_att);
00060 temp.sub(rjoint->abs_pos, pjoint->abs_pos);
00061 rjoint_pos.mul(t_p_att, temp);
00062 rjoint_att.mul(t_p_att, rjoint->abs_att);
00063 rjoint_lin_vel.mul(rjoint_att, rjoint->loc_lin_vel);
00064 rjoint_ang_vel.mul(rjoint_att, rjoint->loc_ang_vel);
00065 t_rjoint_att.tran(rjoint_att);
00066 #ifdef VERBOSE
00067 update_log << rjoint->name << ": rel_lin_vel = " << rjoint->rel_lin_vel << endl;
00068 update_log << rjoint->name << ": rel_ang_vel = " << rjoint->rel_ang_vel << endl;
00069
00070 #endif
00071 std::vector<fVec3> pair_positions;
00072 std::vector<fVec3> pair_normals;
00073 std::vector<fVec3> pair_relvels;
00074 std::vector<fVec3> pair_positions_real;
00075 std::vector<fMat33> pair_orientations;
00076 std::vector<fMat33> pair_orientations_real;
00077 std::vector<fVec3> pair_jdot;
00078 std::vector<fVec3> pair_jdot_real;
00079 for(int m=0; m<n_points; m++)
00080 {
00081 #ifdef VERBOSE
00082 update_log << "- contact point " << m << endl;
00083 #endif
00084
00085
00086
00087
00088 static fVec3 temp;
00089 static fVec3 relpos1, relpos2, relnorm;
00090 double depth;
00091 relpos1(0) = sd_pair->Coord(m)(0);
00092 relpos1(1) = sd_pair->Coord(m)(1);
00093 relpos1(2) = sd_pair->Coord(m)(2);
00094 relpos2.set(relpos1);
00095 relnorm(0) = sd_pair->Normal(m)(0);
00096 relnorm(1) = sd_pair->Normal(m)(1);
00097 relnorm(2) = sd_pair->Normal(m)(2);
00098 relnorm.unit();
00099 depth = sd_pair->Depth(m);
00100 #ifdef VERBOSE
00101 update_log << "p1 = " << relpos1 << ", p2 = " << relpos2 << ", norm = " << relnorm << ", depth = " << depth << endl;
00102 #endif
00103
00104 static fVec3 vel1, vel2, relvel, slipvel;
00105 vel1.cross(pjoint->loc_ang_vel, relpos2);
00106 vel1 += pjoint->loc_lin_vel;
00107 vel2.cross(rjoint_ang_vel, relpos2-rjoint_pos);
00108 vel2 += rjoint_lin_vel;
00109 #ifdef VERBOSE
00110 update_log << "vel1 = " << vel1 << endl;
00111 update_log << "vel2 = " << vel2 << endl;
00112 #endif
00113 relvel.sub(vel2, vel1);
00114 slipvel.set(relvel);
00115 slipvel -= (relnorm*relvel)*relnorm;
00116 #ifdef VERBOSE
00117 update_log << "relvel = " << relvel << endl;
00118 #endif
00119
00120 {
00121 relvel -= (0.005*depth/timestep)*relnorm;
00122
00123 n_total_points++;
00124
00125 double abs_slipvel = slipvel.length();
00126 static fVec3 x, y, z;
00127 static fMat33 ratt;
00128 z.set(relnorm);
00129 if(abs_slipvel > 1e-6)
00130 {
00131 slipvel.unit();
00132 x.set(slipvel);
00133 }
00134 else
00135 {
00136 fVec3 tmp(1,0,0);
00137 slipvel.set(tmp);
00138 slipvel -= (relnorm*tmp)*relnorm;
00139 abs_slipvel = slipvel.length();
00140 if(abs_slipvel > 1e-6)
00141 {
00142 slipvel.unit();
00143 x.set(slipvel);
00144 }
00145 else
00146 {
00147 tmp(0) = 0.0; tmp(1) = 1.0; tmp(2) = 0.0;
00148 slipvel.set(tmp);
00149 slipvel -= (relnorm*tmp)*relnorm;
00150 abs_slipvel = slipvel.length();
00151 slipvel.unit();
00152 x.set(slipvel);
00153 }
00154 }
00155 y.cross(z, x);
00156 ratt(0,0) = x(0); ratt(0,1) = y(0); ratt(0,2) = z(0);
00157 ratt(1,0) = x(1); ratt(1,1) = y(1); ratt(1,2) = z(1);
00158 ratt(2,0) = x(2); ratt(2,1) = y(2); ratt(2,2) = z(2);
00159
00160 static fVec3 relpos2_r;
00161 static fMat33 ratt_r;
00162 temp.sub(relpos2, rjoint_pos);
00163 relpos2_r.mul(t_rjoint_att, temp);
00164 ratt_r.mul(t_rjoint_att, ratt);
00165 pair_positions.push_back(relpos2);
00166 pair_normals.push_back(relnorm);
00167 pair_relvels.push_back(relvel);
00168 #ifdef VERBOSE
00169
00170 #endif
00171 pair_orientations.push_back(ratt);
00172 pair_positions_real.push_back(relpos2_r);
00173 pair_orientations_real.push_back(ratt_r);
00174 static fVec3 omega_x_p, omega_x_omega_x_p, jdot;
00175 omega_x_p.cross(pjoint->rel_ang_vel, relpos2);
00176 omega_x_omega_x_p.cross(pjoint->rel_ang_vel, omega_x_p);
00177 jdot.mul(omega_x_omega_x_p, ratt);
00178 pair_jdot.push_back(jdot);
00179 omega_x_p.cross(rjoint_ang_vel, relpos2);
00180 omega_x_omega_x_p.cross(rjoint_ang_vel, omega_x_p);
00181 jdot.mul(omega_x_omega_x_p, ratt);
00182 pair_jdot_real.push_back(jdot);
00183 }
00184 }
00185
00186 int n_valid_points = pair_relvels.size();
00187 if(n_valid_points == 0) continue;
00188 static char jname[256];
00189 sprintf(jname, "%s_%s", pjoint->name, rjoint->name);
00190 Joint* main_vjoint = new Joint(jname, JFREE);
00191 main_vjoint->real = rjoint;
00192
00193 static fVec3 pjoint_pos;
00194 static fMat33 pjoint_att;
00195 pjoint_att.tran(rjoint_att);
00196 pjoint_pos.mul(pjoint_att, rjoint_pos);
00197 pjoint_pos *= -1.0;
00198 main_vjoint->rpos_real.set(pjoint_pos);
00199 main_vjoint->ratt_real.set(pjoint_att);
00200 AddJoint(main_vjoint, pjoint);
00201 contact_vjoints.push_back(main_vjoint);
00202 std::vector<int> add;
00203 add.resize(n_valid_points);
00204 double min_distance = 1.0e-3;
00205 for(int m=0; m<n_valid_points; m++)
00206 {
00207 add[m] = true;
00208 fVec3& posm = pair_positions[m];
00209 for(int n=0; n<m; n++)
00210 {
00211 if(!add[n]) continue;
00212 fVec3& posn = pair_positions[n];
00213 static fVec3 pp;
00214 pp.sub(posn, posm);
00215 double len = pp.length();
00216
00217 if(len < min_distance)
00218 {
00219 #ifdef VERBOSE
00220 update_log << " " << m << ": too close: " << len << endl;
00221 #endif
00222 add[m] = false;
00223 break;
00224 }
00225 }
00226 }
00227 for(int m=0; m<n_valid_points; m++)
00228 {
00229 if(!add[m]) continue;
00230 fVec3& posm = pair_positions[m];
00231 static fVec3 first_vec;
00232 double min_angle=0.0, max_angle=0.0;
00233 int have_first_vec = false;
00234 for(int n=0; n<n_valid_points; n++)
00235 {
00236 if(n == m || !add[n]) continue;
00237 fVec3& posn = pair_positions[n];
00238 fVec3& norm = pair_normals[n];
00239 static fVec3 pp;
00240 pp.sub(posn, posm);
00241 double len = pp.length();
00242 pp /= len;
00243 if(!have_first_vec)
00244 {
00245 first_vec.set(pp);
00246 have_first_vec = true;
00247 min_angle = 0.0;
00248 max_angle = 0.0;
00249 continue;
00250 }
00251
00252 static fVec3 fcp;
00253 double cs = first_vec * pp;
00254 fcp.cross(first_vec, pp);
00255 double ss = fcp.length();
00256 if(fcp * norm < 0.0) ss *= -1.0;
00257 double angle = atan2(ss, cs);
00258 if(angle < min_angle) min_angle = angle;
00259 if(angle > max_angle) max_angle = angle;
00260 if(max_angle - min_angle >= PI)
00261 {
00262 #ifdef VERBOSE
00263 update_log << " inside: " << max_angle << ", " << min_angle << endl;
00264 #endif
00265 add[m] = false;
00266 break;
00267 }
00268 }
00269 }
00270 for(int m=0; m<n_valid_points; m++)
00271 {
00272 #ifdef VERBOSE
00273 update_log << "check[" << m << "]: pos = " << pair_positions[m] << ", add = " << add[m] << endl;
00274 #endif
00275 if(add[m])
00276 {
00277 static fVec3 loc_relvel;
00278 all_vjoints.push_back(main_vjoint);
00279 loc_relvel.mul(pair_relvels[m], pair_orientations[m]);
00280 contact_relvels.push_back(loc_relvel);
00281 fric_coefs.push_back(sd_pair->SlipFric());
00282
00283 static fMat Jv(3,6), Jr(3,6);
00284 static fMat33 Pcross, RtPx;
00285 fVec3& rpos = pair_positions[m];
00286 static fMat33 t_ratt;
00287 t_ratt.tran(pair_orientations[m]);
00288 Pcross.cross(rpos);
00289 RtPx.mul(t_ratt, Pcross);
00290 Jv(0,0) = t_ratt(0,0);
00291 Jv(0,1) = t_ratt(0,1);
00292 Jv(0,2) = t_ratt(0,2);
00293 Jv(1,0) = t_ratt(1,0);
00294 Jv(1,1) = t_ratt(1,1);
00295 Jv(1,2) = t_ratt(1,2);
00296 Jv(2,0) = t_ratt(2,0);
00297 Jv(2,1) = t_ratt(2,1);
00298 Jv(2,2) = t_ratt(2,2);
00299 Jv(0,3) = -RtPx(0,0);
00300 Jv(0,4) = -RtPx(0,1);
00301 Jv(0,5) = -RtPx(0,2);
00302 Jv(1,3) = -RtPx(1,0);
00303 Jv(1,4) = -RtPx(1,1);
00304 Jv(1,5) = -RtPx(1,2);
00305 Jv(2,3) = -RtPx(2,0);
00306 Jv(2,4) = -RtPx(2,1);
00307 Jv(2,5) = -RtPx(2,2);
00308 #ifdef VERBOSE
00309
00310
00311
00312 #endif
00313 rpos = pair_positions_real[m];
00314 t_ratt.tran(pair_orientations_real[m]);
00315 Pcross.cross(rpos);
00316 RtPx.mul(t_ratt, Pcross);
00317 Jr(0,0) = t_ratt(0,0);
00318 Jr(0,1) = t_ratt(0,1);
00319 Jr(0,2) = t_ratt(0,2);
00320 Jr(1,0) = t_ratt(1,0);
00321 Jr(1,1) = t_ratt(1,1);
00322 Jr(1,2) = t_ratt(1,2);
00323 Jr(2,0) = t_ratt(2,0);
00324 Jr(2,1) = t_ratt(2,1);
00325 Jr(2,2) = t_ratt(2,2);
00326 Jr(0,3) = -RtPx(0,0);
00327 Jr(0,4) = -RtPx(0,1);
00328 Jr(0,5) = -RtPx(0,2);
00329 Jr(1,3) = -RtPx(1,0);
00330 Jr(1,4) = -RtPx(1,1);
00331 Jr(1,5) = -RtPx(1,2);
00332 Jr(2,3) = -RtPx(2,0);
00333 Jr(2,4) = -RtPx(2,1);
00334 Jr(2,5) = -RtPx(2,2);
00335 #ifdef VERBOSE
00336
00337
00338
00339 #endif
00340 all_Jv.push_back(Jv);
00341 all_Jr.push_back(Jr);
00342 all_jdot_v.push_back(pair_jdot[m]);
00343 all_jdot_r.push_back(pair_jdot_real[m]);
00344 }
00345 }
00346 }
00347 init_contact();
00348 in_create_chain = false;
00349
00351 if(n_total_points == 0)
00352 {
00353 pSim::Update();
00354 return 0;
00355 }
00356
00357
00358 CalcPosition();
00359 CalcVelocity();
00360 update_position();
00361 update_velocity();
00362
00363
00364 subchains->calc_contact_force(timestep);
00365
00366
00367 disassembly();
00368 #ifdef USE_MPI
00369
00370 scatter_acc();
00371 #endif
00372
00373
00374 subchains->clear_f_final();
00375 return 0;
00376 }
00377
00378 void pSubChain::clear_f_final()
00379 {
00380 for(int i=0; i<n_outer_joints; i++)
00381 {
00382 outer_joints[i]->f_final.zero();
00383 }
00384 }
00385
00386 int pSubChain::calc_contact_force(double timestep)
00387 {
00388 assert(n_outer_joints/2 == sim->contact_vjoints.size());
00389 int n_contacts = sim->all_vjoints.size();
00390 int n_contacts_3 = 3*n_contacts;
00391 int n_coefs = N_FRIC_CONE_DIV+1;
00392 fMat Phi_hat(n_contacts_3, n_contacts_3);
00393 fVec bias_hat(n_contacts_3);
00394 Phi_hat.zero();
00395 bias_hat.zero();
00396 std::vector<fVec3>& rvels = sim->contact_relvels;
00397 for(int i=0; i<n_contacts; i++)
00398 {
00399 int index = 3*i;
00400 static fVec3 v;
00401 v.set(rvels[i]);
00402 bias_hat(index) = v(0);
00403 bias_hat(index+1) = v(1);
00404 bias_hat(index+2) = v(2);
00405 }
00406
00407 std::vector<int> r_index;
00408 std::vector<int> v_index;
00409 r_index.resize(n_contacts);
00410 v_index.resize(n_contacts);
00411 for(int i=0; i<n_contacts; i++)
00412 {
00413 Joint* vjoint = sim->all_vjoints[i];
00414 for(int j=0; j<n_outer_joints; j++)
00415 {
00416 if(outer_joints[j] == sim->joint_info[vjoint->i_joint].pjoints[0])
00417 {
00418 r_index[i] = j;
00419 }
00420 if(outer_joints[j] == sim->joint_info[vjoint->i_joint].pjoints[1])
00421 {
00422 v_index[i] = j;
00423 }
00424 }
00425
00426 }
00427 for(int i=0; i<n_contacts; i++)
00428 {
00429 int i3 = 3*i;
00430
00431 static fVec dacc_r(3), dacc_v(3);
00432
00433 dacc_r.mul(sim->all_Jv[i], acc_temp[r_index[i]]);
00434 dacc_v.mul(sim->all_Jv[i], acc_temp[v_index[i]]);
00435 #if 1
00436 dacc_r(0) += sim->all_jdot_r[i](0);
00437 dacc_r(1) += sim->all_jdot_r[i](1);
00438 dacc_r(2) += sim->all_jdot_r[i](2);
00439 dacc_v(0) += sim->all_jdot_v[i](0);
00440 dacc_v(1) += sim->all_jdot_v[i](1);
00441 dacc_v(2) += sim->all_jdot_v[i](2);
00442 #endif
00443 dacc_r -= dacc_v;
00444 dacc_r *= timestep;
00445 bias_hat(i3) += dacc_r(0);
00446 bias_hat(i3+1) += dacc_r(1);
00447 bias_hat(i3+2) += dacc_r(2);
00448 #ifdef VERBOSE
00449
00450 #endif
00451
00452 for(int j=0; j<n_contacts; j++)
00453 {
00454 int j3 = 3*j;
00455 static fMat PJ(6,3), JPJ(3,3), L(3,3);
00456 L.zero();
00457 PJ.mul(Lambda[r_index[i]][r_index[j]], tran(sim->all_Jv[j]));
00458 JPJ.mul(sim->all_Jv[i], PJ);
00459
00460
00461
00462
00463
00464 L += JPJ;
00465 PJ.mul(Lambda[v_index[i]][r_index[j]], tran(sim->all_Jv[j]));
00466 JPJ.mul(sim->all_Jv[i], PJ);
00467 L -= JPJ;
00468 PJ.mul(Lambda[r_index[i]][v_index[j]], tran(sim->all_Jv[j]));
00469 JPJ.mul(sim->all_Jv[i], PJ);
00470 L -= JPJ;
00471 PJ.mul(Lambda[v_index[i]][v_index[j]], tran(sim->all_Jv[j]));
00472 JPJ.mul(sim->all_Jv[i], PJ);
00473 L += JPJ;
00474 #ifdef VERBOSE
00475
00476 #endif
00477 Phi_hat.set_submat(i3, j3, L);
00478 #if 0
00479 static fMat Lsub(3,3);
00480 Lsub.get_submat(i3, j3, Phi_hat);
00481 update_log << "Phi_hat[" << i << "][" << j << "] = " << Lsub << endl;
00482 #if 1
00483 Lsub.get_submat(0, 0, Lambda[r_index[i]][r_index[j]]);
00484 update_log << "L[" << r_index[i] << "][" << r_index[j] << "] = " << Lsub << endl;
00485 Lsub.get_submat(0, 0, Lambda[v_index[i]][r_index[j]]);
00486 update_log << "L[" << v_index[i] << "][" << r_index[j] << "] = " << Lsub << endl;
00487 Lsub.get_submat(0, 0, Lambda[r_index[i]][v_index[j]]);
00488 update_log << "L[" << r_index[i] << "][" << v_index[j] << "] = " << Lsub << endl;
00489 Lsub.get_submat(0, 0, Lambda[v_index[i]][v_index[j]]);
00490 update_log << "L[" << v_index[i] << "][" << v_index[j] << "] = " << Lsub << endl;
00491 #endif
00492 #endif
00493 }
00494 }
00495 #ifdef VERBOSE
00496
00497
00498 #endif
00499 Phi_hat *= timestep;
00500
00501 int n_total_coef = n_coefs*n_contacts + n_contacts;
00502 fMat N(n_total_coef, n_total_coef);
00503 fMat Ck(3, n_coefs);
00504 fMat E(n_coefs, 1);
00505 fMat* Ehat_t = new fMat [n_contacts];
00506 fVec r(n_total_coef);
00507
00508 Ck.zero();
00509 Ck(2, 0) = 1.0;
00510 E.zero();
00511 for(int m=0; m<N_FRIC_CONE_DIV; m++)
00512 {
00513 Ck(0, m+1) = sim->cone_dir[m](0);
00514 Ck(1, m+1) = sim->cone_dir[m](1);
00515 E(m+1, 0) = 1.0;
00516 }
00517 for(int i=0; i<n_contacts; i++)
00518 {
00519 double fric_coef = sim->fric_coefs[i];
00520 Ehat_t[i].resize(1, n_coefs);
00521 Ehat_t[i](0,0) = fric_coef;
00522 for(int m=0; m<N_FRIC_CONE_DIV; m++)
00523 {
00524 Ehat_t[i](0, m+1) = -1.0;
00525 }
00526 }
00527 N.zero();
00528 r.zero();
00529 for(int i=0; i<n_contacts; i++)
00530 {
00531 static fMat N_ij(n_coefs,n_coefs);
00532 static fVec r_i(n_coefs);
00533 int i3 = i*3, iN = i*n_coefs;
00534 r_i.zero();
00535 for(int j=0; j<n_contacts; j++)
00536 {
00537 static fMat Phi_ij(3,3), CP(n_coefs,3);
00538 static fMat B_ij(n_coefs,n_coefs), CinvP(n_coefs, 3);
00539
00540 int j3 = j*3, jN = j*(n_coefs);
00541 Phi_ij.get_submat(i3, j3, Phi_hat);
00542 CP.mul(tran(Ck), Phi_ij);
00543 N_ij.mul(CP, Ck);
00544 N.set_submat(iN, jN, N_ij);
00545 }
00546
00547 static fVec bias_i(3);
00548 bias_i.get_subvec(i3, bias_hat);
00549 r_i.mul(bias_i, Ck);
00550 r.set_subvec(iN, r_i);
00551
00552 N.set_submat(iN, n_coefs*n_contacts+i, E);
00553 N.set_submat(n_coefs*n_contacts+i, iN, Ehat_t[i]);
00554 }
00555
00556 double max_error = 1.0e-3;
00557
00558 int max_iteration = 10000;
00559
00560 std::vector<int> presolve_g2w;
00561 int presolve_failed = false;
00562 fVec presolve_g2;
00563 presolve_g2w.resize(n_contacts);
00564 for(int i=0; i<n_contacts; i++)
00565 {
00566 presolve_g2w[i] = -1;
00567 }
00568 {
00569 fMat Ns(n_contacts, n_contacts);
00570 fVec rs(n_contacts), as;
00571 int n_iteration = 0;
00572 for(int i=0; i<n_contacts; i++)
00573 {
00574 rs(i) = bias_hat(i*3+2);
00575 for(int j=0; j<n_contacts; j++)
00576 {
00577 Ns(i, j) = Phi_hat(i*3+2, j*3+2);
00578 }
00579 }
00580 LCP lcp_s(Ns, rs);
00581 presolve_failed = lcp_s.SolvePivot2(presolve_g2, as, max_error, max_iteration, &n_iteration, presolve_g2w);
00582 if(presolve_failed)
00583 {
00584 cerr << "presolve_failed (" << n_iteration << "/" << max_iteration << ")" << endl;
00585 cerr << "Phi_hat = " << Phi_hat << endl;
00586 }
00587 #ifdef VERBOSE
00588 for(int i=0; i<n_contacts; i++)
00589 {
00590 update_log << "presolve_g2w[" << i << "] = " << presolve_g2w[i] << endl;
00591 }
00592 #endif
00593 #ifdef MEASURE_COLLISION_TIME
00594 sim->n_total_nodes += n_iteration;
00595 #endif
00596 }
00597 #ifdef USE_INITIAL_GUESS
00598 std::vector<int> w2a, w2g, z2a, z2g;
00599 #endif
00600
00601 std::vector<int> active2all;
00602 fVec* fk = new fVec [n_contacts];
00603 fVec* vk = new fVec [n_contacts];
00604 fMat N_active;
00605 fVec r_active;
00606 #if 1
00607 if(!presolve_failed)
00608 {
00609 for(int i=0; i<n_contacts; i++)
00610 {
00611 if(presolve_g2w[i] >= 0)
00612 {
00613 active2all.push_back(i);
00614 }
00615 }
00616 }
00617 else
00618 #endif
00619 {
00620 for(int i=0; i<n_contacts; i++)
00621 {
00622 active2all.push_back(i);
00623 }
00624 }
00625 #ifdef VERBOSE
00626 update_log << "*** start searching constraint set" << endl;
00627 #endif
00628 int max_global_iteration = 1;
00629 int count = 0;
00630 while(count < max_global_iteration)
00631 {
00632 std::vector<int> all2active;
00633 std::vector<int> g2w;
00634 all2active.resize(n_contacts);
00635 for(int i=0; i<n_contacts; i++)
00636 {
00637 all2active[i] = -1;
00638 fk[i].resize(3);
00639 fk[i].zero();
00640 }
00641 int n_active_contacts = active2all.size();
00642 if(n_active_contacts > 0)
00643 {
00644 int n_active_coef = n_active_contacts*n_coefs + n_active_contacts;
00645 N_active.resize(n_active_coef, n_active_coef);
00646 r_active.resize(n_active_coef);
00647 N_active.zero();
00648 r_active.zero();
00649 for(int i=0; i<n_active_contacts; i++)
00650 {
00651 all2active[active2all[i]] = i;
00652 }
00653 #ifdef VERBOSE
00654 update_log << "n_active_contacts = " << n_active_contacts << endl;
00655 update_log << "n_active_coef = " << n_active_coef << endl;
00656 #endif
00657 for(int i=0; i<n_active_contacts; i++)
00658 {
00659 static fMat N_ij(n_coefs,n_coefs);
00660 static fVec r_i(n_coefs);
00661 int i_active_N = i*n_coefs;
00662 int i_all = active2all[i];
00663 int i_all_N = i_all*n_coefs;
00664 for(int j=0; j<n_active_contacts; j++)
00665 {
00666 int j_all = active2all[j];
00667 int j_all_N = j_all*n_coefs;
00668 int j_active_N = j*n_coefs;
00669 N_ij.get_submat(i_all_N, j_all_N, N);
00670 N_active.set_submat(i_active_N, j_active_N, N_ij);
00671 }
00672 r_i.get_subvec(i_all_N, r);
00673 r_active.set_subvec(i_active_N, r_i);
00674 N_active.set_submat(i_active_N, n_coefs*n_active_contacts+i, E);
00675 N_active.set_submat(n_coefs*n_active_contacts+i, i_active_N, Ehat_t[i]);
00676 }
00677 #ifdef USE_INITIAL_GUESS
00678 static fMat N_init;
00679 static fVec r_init;
00680 N_init.resize(n_active_coef, n_active_coef);
00681 r_init.resize(n_active_coef);
00682 w2a.resize(n_active_coef);
00683 w2g.resize(n_active_coef);
00684 z2a.resize(n_active_coef);
00685 z2g.resize(n_active_coef);
00686 for(int i=0; i<n_active_coef; i++)
00687 {
00688 w2a[i] = i;
00689 w2g[i] = -1;
00690 z2a[i] = -1;
00691 z2g[i] = i;
00692 }
00693 for(int i=0; i<n_active_contacts; i++)
00694 {
00695 int idx = i*n_coefs;
00696 w2a[idx] = -1;
00697 w2g[idx] = idx;
00698 z2a[idx] = idx;
00699 z2g[idx] = -1;
00700 }
00701 LCP::Pivot(w2a, w2g, z2a, z2g, N_active, r_active, N_init, r_init);
00702 N_active.set(N_init);
00703 r_active.set(r_init);
00704 #endif
00705 LCP lcp(N_active, r_active);
00706
00707 static fVec g2, a;
00708 int n_iteration = 0;
00709 g2w.resize(n_active_coef);
00710 #ifdef MEASURE_COLLISION_TIME
00711 sim->n_total_contacts += n_contacts;
00712 sim->n_active_contacts += n_active_contacts;
00713 LongInteger t1 = GetTick();
00714 #endif
00715 #ifdef USE_NORMAL_LEMKE
00716 int failed = lcp.SolvePivot(g2, a, max_error, max_iteration, &n_iteration, g2w);
00717 #else
00718 int failed = lcp.SolvePivot2(g2, a, max_error, max_iteration, &n_iteration, g2w);
00719 #endif
00720 count++;
00721 #ifdef MEASURE_COLLISION_TIME
00722 LongInteger t2 = GetTick();
00723 sim->lcp_solve_time += ExpiredTime(t1, t2);
00724 sim->n_total_nodes += n_iteration;
00725 sim->n_loops += lcp.NumLoops();
00726 sim->n_errors += lcp.NumErrors();
00727 #endif
00728 #ifdef VERBOSE
00729 update_log << "n_iteration = " << n_iteration << endl;
00730 #endif
00731 fVec error(n_active_coef);
00732 error.mul(N_active, g2);
00733 error += r_active;
00734 error -= a;
00735 #ifdef VERBOSE
00736 update_log << "error = " << error.length() << endl;
00737 #endif
00738 if(error.length() > 1e-4) failed = true;
00739 if(failed)
00740 {
00741 cerr << "failed (" << n_iteration << "/" << max_iteration << "): n_contacts = " << n_contacts << endl;
00742 #ifdef MEASURE_COLLISION_TIME
00743 sim->n_failure_frames++;
00744 #endif
00745 #ifdef VERBOSE
00746 update_log << "failed" << endl;
00747 #endif
00748 if(!presolve_failed)
00749 {
00750 cerr << "using presolve result" << endl;
00751 for(int i=0; i<n_contacts; i++)
00752 {
00753 fk[i](2) = presolve_g2(i);
00754 }
00755 }
00756 break;
00757 }
00758 else
00759 {
00760 #ifdef VERBOSE
00761 update_log << "success" << endl;
00762 update_log << "g2 = " << tran(g2) << endl;
00763 update_log << "a = " << tran(a) << endl;
00764 update_log << "g2w = " << endl;
00765 for(int i=0; i<n_active_contacts; i++)
00766 {
00767 update_log << "active contact[" << i << "] a = " << flush;
00768 for(int j=0; j<n_coefs; j++)
00769 {
00770 update_log << g2w[i*n_coefs+j] << " " << flush;
00771 }
00772 update_log << endl;
00773 }
00774 for(int i=0; i<n_active_contacts; i++)
00775 {
00776 update_log << "active contact[" << i << "] lambda = " << g2w[n_active_contacts*n_coefs+i] << endl;
00777 }
00778 #endif
00779 #ifdef USE_INITIAL_GUESS
00780 for(int i=0; i<n_active_coef; i++)
00781 {
00782 if(w2g[i] >= 0)
00783 {
00784 g2(i) = a(w2g[i]);
00785 }
00786 }
00787 #endif
00788 for(int i=0; i<n_active_contacts; i++)
00789 {
00790 static fVec g2_i(n_coefs);
00791 int iN = i*n_coefs;
00792 g2_i.get_subvec(iN, g2);
00793 fk[active2all[i]].mul(Ck, g2_i);
00794 #ifdef VERBOSE
00795 update_log << "fk[" << active2all[i] << "] = " << tran(fk[active2all[i]]) << endl;
00796 #endif
00797 }
00798 }
00799 active2all.clear();
00800 }
00801
00802 for(int i=0; i<n_contacts; i++)
00803 {
00804 vk[i].resize(3);
00805 vk[i].zero();
00806 }
00807 int constraint_modified = false;
00808 for(int i=0; i<n_contacts; i++)
00809 {
00810 static fVec bias_i(3);
00811 bias_i.get_subvec(3*i, bias_hat);
00812 for(int j=0; j<n_contacts; j++)
00813 {
00814 static fMat Phi_ij(3,3);
00815 Phi_ij.get_submat(3*i, 3*j, Phi_hat);
00816 vk[i] += Phi_ij * fk[j];
00817 }
00818 vk[i] += bias_i;
00819 #ifdef VERBOSE
00820 update_log << "vk[" << i << "] = " << tran(vk[i]) << endl;
00821 if(vk[i](2) < -max_error)
00822 {
00823 update_log << "--- too small vertical velocity" << endl;
00824 }
00825 #endif
00826 #ifdef USE_INITIAL_GUESS
00827 if((all2active[i] >= 0 && w2g[all2active[i]*n_coefs] < 0 && g2w[all2active[i]*n_coefs] >= 0) ||
00828 (all2active[i] >= 0 && w2g[all2active[i]*n_coefs] >= 0 && g2w[all2active[i]*n_coefs] < 0) ||
00829 #else
00830 if((all2active[i] >= 0 && g2w[all2active[i]*n_coefs] >= 0) ||
00831 #endif
00832 vk[i](2) < -max_error)
00833 {
00834 active2all.push_back(i);
00835 #ifdef VERBOSE
00836 update_log << "added to active" << endl;
00837 #endif
00838 if(vk[i](2) < -max_error)
00839 {
00840 constraint_modified = true;
00841 #ifdef VERBOSE
00842 update_log << "constraint modified" << endl;
00843 #endif
00844 }
00845 }
00846 #ifdef VERBOSE
00847 else
00848 #ifdef USE_INITIAL_GUESS
00849 if((all2active[i] >= 0 && w2g[all2active[i]*n_coefs] < 0 && g2w[all2active[i]*n_coefs] >= 0) ||
00850 (all2active[i] >= 0 && w2g[all2active[i]*n_coefs] >= 0 && g2w[all2active[i]*n_coefs] < 0))
00851 #else
00852 if(all2active[i] >= 0 && g2w[all2active[i]*n_coefs] >= 0)
00853 #endif
00854 {
00855 update_log << "removed from active" << endl;
00856 }
00857 #endif
00858 }
00859 if(!constraint_modified) break;
00860 }
00861 #ifdef VERBOSE
00862 update_log << "end of search" << endl;
00863 #endif
00864
00865
00866 for(int i=0; i<n_outer_joints; i++)
00867 {
00868 outer_joints[i]->f_final.zero();
00869 }
00870 for(int i=0; i<n_contacts; i++)
00871 {
00872 static fVec3 f, f_virt, f_real, moment;
00873 f(0) = fk[i](0);
00874 f(1) = fk[i](1);
00875 f(2) = fk[i](2);
00876 Joint* r_joint = outer_joints[r_index[i]]->joint;
00877 Joint* v_joint = outer_joints[v_index[i]]->joint;
00878 static fVec f0(6);
00879 f0.mul(fk[i], sim->all_Jr[i]);
00880 r_joint->real->ext_force(0) += f0(0);
00881 r_joint->real->ext_force(1) += f0(1);
00882 r_joint->real->ext_force(2) += f0(2);
00883 r_joint->real->ext_moment(0) += f0(3);
00884 r_joint->real->ext_moment(1) += f0(4);
00885 r_joint->real->ext_moment(2) += f0(5);
00886 f0.mul(fk[i], sim->all_Jv[i]);
00887 v_joint->parent->ext_force(0) -= f0(0);
00888 v_joint->parent->ext_force(1) -= f0(1);
00889 v_joint->parent->ext_force(2) -= f0(2);
00890 v_joint->parent->ext_moment(0) -= f0(3);
00891 v_joint->parent->ext_moment(1) -= f0(4);
00892 v_joint->parent->ext_moment(2) -= f0(5);
00893 outer_joints[r_index[i]]->f_final += f0;
00894 outer_joints[v_index[i]]->f_final -= f0;
00895 #ifdef VERBOSE
00896 update_log << r_joint->real->name << " force/moment: " << r_joint->real->ext_force << r_joint->real->ext_moment << endl;
00897 update_log << v_joint->parent->name << " force/moment: " << v_joint->parent->ext_force << v_joint->parent->ext_moment << endl;
00898 #endif
00899 }
00900 delete[] vk;
00901 delete[] fk;
00902 delete[] Ehat_t;
00903 return 0;
00904 }
00905
00906