CVelocityLimiter(dBodyID b,float l,float yl)
{
R_ASSERT(b);
m_body=b;
dVectorSet(m_safe_velocity,dBodyGetLinearVel(m_body));
dVectorSet(m_safe_position,dBodyGetPosition(m_body));
l_limit=l;
y_limit=yl;
}
virtual void PhDataUpdate(dReal step)
{
const float *linear_velocity =dBodyGetLinearVel(m_body);
if(VelocityLimit())
{
dBodySetPosition(m_body,
m_safe_position[0]+linear_velocity[0]*fixed_step,
m_safe_position[1]+linear_velocity[1]*fixed_step,
m_safe_position[2]+linear_velocity[2]*fixed_step);
}
if(!dV_valid(dBodyGetPosition(m_body)))
dBodySetPosition(m_body,m_safe_position[0]-m_safe_velocity[0]*fixed_step,
m_safe_position[1]-m_safe_velocity[1]*fixed_step,
m_safe_position[2]-m_safe_velocity[2]*fixed_step);
dVectorSet(m_safe_position,dBodyGetPosition(m_body));
dVectorSet(m_safe_velocity,linear_velocity);
}
shell_root CPHShellSplitterHolder::ElementSingleSplit(const element_fracture &split_elem,const CPHElement* source_element)
{
//const CPHShellSplitter& splitter=m_splitters[aspl];
//CPHElement* element=m_pShell->elements[splitter.m_element];
CPhysicsShell *new_shell_last=P_create_Shell();
CPHShell *new_shell_last_desc=smart_cast<CPHShell*>(new_shell_last);
new_shell_last->mXFORM.set(m_pShell->mXFORM); const u16 start_joint=split_elem.second.m_start_jt_num;
R_ASSERT(_valid(new_shell_last->mXFORM));
const u16 end_joint=split_elem.second.m_end_jt_num;
//it is not right for multiple joints attached to the unsplited part becource all these need to be reattached
if(start_joint!=end_joint)
{
JOINT_STORAGE& joints=m_pShell->joints;
JOINT_I i=joints.begin()+ start_joint,e=joints.begin()+ end_joint;
for(;i!=e;++i)
{
CPHJoint* joint=(*i);
if(joint->PFirst_element()==source_element)
{
IKinematics* K = m_pShell->PKinematics();
dVector3 safe_pos1, safe_pos2;
dQuaternion safe_q1, safe_q2;
CPhysicsElement* el1=cast_PhysicsElement(split_elem.first),*el2=joint->PSecond_element();
dBodyID body1=el1->get_body(), body2=el2->get_body();
dVectorSet(safe_pos1,dBodyGetPosition(body1));
dVectorSet(safe_pos2,dBodyGetPosition(body2));
dQuaternionSet(safe_q1,dBodyGetQuaternion(body1));
dQuaternionSet(safe_q2,dBodyGetQuaternion(body2));
//m_pShell->PlaceBindToElForms();
K->LL_GetBindTransform(bones_bind_forms);
el1->SetTransform(bones_bind_forms[el1->m_SelfID]);
el2->SetTransform(bones_bind_forms[el2->m_SelfID]);
joint->ReattachFirstElement(split_elem.first);
dVectorSet(const_cast<dReal*>(dBodyGetPosition(body1)),safe_pos1);
dVectorSet(const_cast<dReal*>(dBodyGetPosition(body2)),safe_pos2);
dQuaternionSet(const_cast<dReal*>(dBodyGetQuaternion(body1)),safe_q1);
dQuaternionSet(const_cast<dReal*>(dBodyGetQuaternion(body2)),safe_q2);
dBodySetPosition(body1,safe_pos1[0],safe_pos1[1],safe_pos1[2]);
dBodySetPosition(body2,safe_pos2[0],safe_pos2[1],safe_pos2[2]);
dBodySetQuaternion(body1,safe_q1);
dBodySetQuaternion(body2,safe_q2);
}
}
// m_pShell->joints[split_elem.second.m_start_jt_num]->ReattachFirstElement(split_elem.first);
}
//the last new shell will have all splitted old elements end joints and one new element reattached to old joint
//m_splitters.erase(m_splitters.begin()+aspl);
//now aspl points to the next splitter
if((split_elem.first)->FracturesHolder())//if this element can be splitted add a splitter for it
new_shell_last_desc->AddSplitter(CPHShellSplitter::splElement,0,u16(-1));//
new_shell_last_desc->add_Element(split_elem.first);
//pass splitters taking into account that one element was olready added
PassEndSplitters(split_elem.second,new_shell_last_desc,0,0);
InitNewShell(new_shell_last_desc);
m_pShell->PassEndElements(split_elem.second.m_start_el_num,split_elem.second.m_end_el_num,new_shell_last_desc);
m_pShell->PassEndJoints(split_elem.second.m_start_jt_num,split_elem.second.m_end_jt_num,new_shell_last_desc);
new_shell_last_desc->set_PhysicsRefObject(0);
///////////////////temporary for initialization set old Kinematics in new shell/////////////////
new_shell_last->set_Kinematics(m_pShell->PKinematics());
new_shell_last_desc->AfterSetActive();
new_shell_last->set_Kinematics(NULL);
VERIFY2(split_elem.second.m_bone_id<64,"strange root");
VERIFY(_valid(new_shell_last->mXFORM));
VERIFY(dBodyStateValide(source_element->get_bodyConst()));
VERIFY(dBodyStateValide(split_elem.first->get_body()));
new_shell_last->set_ObjectContactCallback(NULL);
new_shell_last->set_PhysicsRefObject(NULL);
return mk_pair(new_shell_last,split_elem.second.m_bone_id);
}
