int pSim::clear_contact()
{
int n_contacts = contact_vjoints.size();
if(n_contacts == 0) return 0;
int org_n_joint = n_joint;
JointInfo* jinfo_save = joint_info;
for(joint_list::iterator j=contact_vjoints.begin(); j!=contact_vjoints.end(); j++)
{
RemoveJoint(*j);
delete *j;
}
// clear and init Chain's data
Chain::clear_data();
#ifdef SEGA
Chain::init();
#else
Chain::init(0);
#endif
joint_info = new JointInfo [n_joint];
for(int i=0; i<org_n_joint; i++)
{
if(contact_vjoint_index(jinfo_save[i].pjoints[0]->joint) < 0)
{
Joint* j = jinfo_save[i].pjoints[0]->joint;
joint_info[j->i_joint].pjoints[0] = jinfo_save[i].pjoints[0];
joint_info[j->i_joint].pjoints[1] = jinfo_save[i].pjoints[1];
joint_info[j->i_joint].plink = jinfo_save[i].plink;
}
else
{
delete jinfo_save[i].pjoints[0];
delete jinfo_save[i].pjoints[1];
if(jinfo_save[i].plink) delete jinfo_save[i].plink;
}
}
if(jinfo_save) delete[] jinfo_save;
contact_vjoints.clear();
contact_relvels.clear();
fric_coefs.clear();
all_vjoints.clear();
all_Jv.clear();
all_Jr.clear();
all_jdot_v.clear();
all_jdot_r.clear();
return 0;
}
//------------------------------------------------------------------------------
void QSim::_destroyOnlyJoint(QJoint* joint)
{
RemoveJoint(joint);
delete joint;
}