Joint* internal_getCommonJoint(dBodyID body0, dBodyID body1)
{
// First we need to find the ODE joint connecting the bodies
// (it would be ideal if ODE included this functionality...).
// We only need to check one of the bodies' ODE joints
// because it is assumed here that the two bodies are
// connected, thus they should have a common ODE joint.
int numJoints0 = dBodyGetNumJoints(body0);
dJointID theJoint = 0;
// Loop through body0's ODE joints.
int i = 0;
for (i = 0; i < numJoints0; ++i)
{
dJointID currentJoint = dBodyGetJoint(body0, i);
dBodyID jointBody0 = dJointGetBody(currentJoint, 0);
dBodyID jointBody1 = dJointGetBody(currentJoint, 1);
if ((jointBody0 == body0 && jointBody1 == body1) ||
(jointBody0 == body1 && jointBody1 == body0))
{
// This is the ODE joint connecting the two bodies.
// Note that if the two bodies are connected by multiple
// Joints, the behavior is undefined.
theJoint = currentJoint;
}
}
// Make sure the ODE joint was actually found. This should
// be guaranteed.
assert(theJoint);
// Now return the associated OPAL Joint.
return (Joint*) dJointGetData(theJoint);
}
void CPHFracturesHolder::PhTune(dBodyID body)
{
//iterate through all body's joints and set joints feedbacks where is not already set
//contact feedbacks stored in global storage - ContactFeedBacks wich cleared on each step
//breacable joints already has their feedbacks,
//feedbacks for rest noncontact joints stored in m_feedbacks in runtime in this function and
//and killed by destructor
//int dBodyGetNumJoints (dBodyID b);
//dJointID dBodyGetJoint (dBodyID, int index);
//dJointGetType
//dJointTypeContact
int num=dBodyGetNumJoints(body);
for(int i=0;i<num;++i)
{
dJointID joint=dBodyGetJoint(body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointSetFeedback(joint,ContactFeedBacks.add());
}
else
{
CPHJoint* ph_joint=(CPHJoint*)dJointGetData(joint);
if(!(ph_joint&&ph_joint->JointDestroyInfo())) dJointSetFeedback(joint,ContactFeedBacks.add());
//if(!dJointGetFeedback(joint))
//{
// m_feedbacks.push_back(dJointFeedback());
// dJointSetFeedback(joint,&m_feedbacks.back());
//}
}
}
}
void* PhysicsJoint::getData( void)
{
return dJointGetData(_JointID);
}
//#define DBG_BREAK
bool CPHFracture::Update(CPHElement* element)
{
////itterate through impacts & calculate
dBodyID body=element->get_body();
//const Fvector& v_bodyvel=*((Fvector*)dBodyGetLinearVel(body));
CPHFracturesHolder* holder=element->FracturesHolder();
PH_IMPACT_STORAGE& impacts=holder->Impacts();
Fvector second_part_force,first_part_force,second_part_torque,first_part_torque;
second_part_force.set(0.f,0.f,0.f);
first_part_force.set(0.f,0.f,0.f);
second_part_torque.set(0.f,0.f,0.f);
first_part_torque.set(0.f,0.f,0.f);
//const Fvector& body_local_pos=element->local_mass_Center();
const Fvector& body_global_pos=*(const Fvector*)dBodyGetPosition(body);
Fvector body_to_first, body_to_second;
body_to_first.set(*((const Fvector*)m_firstM.c));//,body_local_pos
body_to_second.set(*((const Fvector*)m_secondM.c));//,body_local_pos
//float body_to_first_smag=body_to_first.square_magnitude();
//float body_to_second_smag=body_to_second.square_magnitude();
int num=dBodyGetNumJoints(body);
for(int i=0;i<num;i++)
{
bool applied_to_second=false;
dJointID joint=dBodyGetJoint(body,i);
dJointFeedback* feedback=dJointGetFeedback(joint);
VERIFY2(feedback,"Feedback was not set!!!");
dxJoint* b_joint=(dxJoint*) joint;
bool b_body_second=(b_joint->node[1].body==body);
Fvector joint_position;
if(dJointGetType(joint)==dJointTypeContact)
{
dxJointContact* c_joint=(dxJointContact*)joint;
dGeomID first_geom=c_joint->contact.geom.g1;
dGeomID second_geom=c_joint->contact.geom.g2;
joint_position.set(*(Fvector*)c_joint->contact.geom.pos);
if(dGeomGetClass(first_geom)==dGeomTransformClass)
{
first_geom=dGeomTransformGetGeom(first_geom);
}
if(dGeomGetClass(second_geom)==dGeomTransformClass)
{
second_geom=dGeomTransformGetGeom(second_geom);
}
dxGeomUserData* UserData;
UserData=dGeomGetUserData(first_geom);
if(UserData)
{
u16 el_position=UserData->element_position;
//define if the contact applied to second part;
if(el_position<element->numberOfGeoms()&&
el_position>=m_start_geom_num&&
el_position<m_end_geom_num&&
first_geom==element->Geom(el_position)->geometry()
) applied_to_second=true;
}
UserData=dGeomGetUserData(second_geom);
if(UserData)
{
u16 el_position=UserData->element_position;
if(el_position<element->numberOfGeoms()&&
el_position>=m_start_geom_num&&
el_position<m_end_geom_num&&
second_geom==element->Geom(el_position)->geometry()
) applied_to_second=true;
}
}
else
{
CPHJoint* J = (CPHJoint*) dJointGetData(joint);
if(!J)continue;//hack..
J->PSecondElement()->InterpolateGlobalPosition(&joint_position);
CODEGeom* root_geom=J->RootGeom();
if(root_geom)
{
u16 el_position=root_geom->element_position();
if(element==J->PFirst_element()&&
el_position<element->numberOfGeoms()&&
el_position>=m_start_geom_num&&
el_position<m_end_geom_num
) applied_to_second=true;
}
}
//accomulate forces applied by joints to first and second parts
Fvector body_to_joint;
body_to_joint.sub(joint_position,body_global_pos);
if(applied_to_second)
{
Fvector shoulder;
shoulder.sub(body_to_joint,body_to_second);
if(b_body_second)
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f2);
second_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
second_part_torque.add(torque);
}
else
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f1);
second_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
second_part_torque.add(torque);
}
}
else
{
Fvector shoulder;
shoulder.sub(body_to_joint,body_to_first);
if(b_body_second)
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f2);
first_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
first_part_torque.add(torque);
}
else
{
Fvector joint_force;
joint_force.set(*(const Fvector*)feedback->f1);
first_part_force.add(joint_force);
Fvector torque;
torque.crossproduct(shoulder,joint_force);
first_part_torque.add(torque);
}
}
}
PH_IMPACT_I i_i=impacts.begin(),i_e=impacts.end();
for(;i_i!=i_e;i_i++)
{
u16 geom = i_i->geom;
if((geom>=m_start_geom_num&&geom<m_end_geom_num))
{
Fvector force;
force.set(i_i->force);
force.mul(ph_console::phRigidBreakWeaponFactor);
Fvector second_to_point;
second_to_point.sub(body_to_second,i_i->point);
//force.mul(30.f);
second_part_force.add(force);
Fvector torque;
torque.crossproduct(second_to_point,force);
second_part_torque.add(torque);
}
else
{
Fvector force;
force.set(i_i->force);
Fvector first_to_point;
first_to_point.sub(body_to_first,i_i->point);
//force.mul(4.f);
first_part_force.add(force);
Fvector torque;
torque.crossproduct(first_to_point,force);
second_part_torque.add(torque);
}
}
Fvector gravity_force;
gravity_force.set(0.f,-ph_world->Gravity()*m_firstM.mass,0.f);
first_part_force.add(gravity_force);
second_part_force.add(gravity_force);
dMatrix3 glI1,glI2,glInvI,tmp;
// compute inertia tensors in global frame
dMULTIPLY2_333 (tmp,body->invI,body->R);
dMULTIPLY0_333 (glInvI,body->R,tmp);
dMULTIPLY2_333 (tmp,m_firstM.I,body->R);
dMULTIPLY0_333 (glI1,body->R,tmp);
dMULTIPLY2_333 (tmp,m_secondM.I,body->R);
dMULTIPLY0_333 (glI2,body->R,tmp);
//both parts have eqiual start angular vel same as have body so we ignore it
//compute breaking torque
///break_torque=glI2*glInvI*first_part_torque-glI1*glInvI*second_part_torque+crossproduct(second_in_bone,second_part_force)-crossproduct(first_in_bone,first_part_force)
Fvector break_torque,vtemp;
dMULTIPLY0_331 ((float*)&break_torque,glInvI,(float*)&first_part_torque);
dMULTIPLY0_331 ((float*)&break_torque,glI2,(float*)&break_torque);
dMULTIPLY0_331 ((float*)&vtemp,glInvI,(float*)&second_part_torque);
dMULTIPLY0_331 ((float*)&vtemp,glI1,(float*)&vtemp);
break_torque.sub(vtemp);
//Fvector first_in_bone,second_in_bone;
//first_in_bone.sub(*((const Fvector*)m_firstM.c),m_pos_in_element);
//second_in_bone.sub(*((const Fvector*)m_secondM.c),m_pos_in_element);
//vtemp.crossproduct(second_in_bone,second_part_force);
//break_torque.add(vtemp);
//vtemp.crossproduct(first_in_bone,first_part_force);
//break_torque.sub(vtemp);
#ifdef DBG_BREAK
float btm_dbg=break_torque.magnitude()*ph_console::phBreakCommonFactor/torque_factor;
#endif
if(break_torque.magnitude()*ph_console::phBreakCommonFactor>m_break_torque*torque_factor)
{
//m_break_torque.set(second_part_torque);
m_pos_in_element.set(second_part_force);
m_break_force=second_part_torque.x;
m_break_torque=second_part_torque.y;
m_add_torque_z=second_part_torque.z;
m_breaked=true;
#ifndef DBG_BREAK
return m_breaked;
#endif
}
Fvector break_force;//=1/(m1+m2)*(F1*m2-F2*m1)+r2xT2/(r2^2)-r1xT1/(r1^2)
break_force.set(first_part_force);
break_force.mul(m_secondM.mass);
vtemp.set(second_part_force);
vtemp.mul(m_firstM.mass);
break_force.sub(vtemp);
break_force.mul(1.f/element->getMass());//element->getMass()//body->mass.mass
//vtemp.crossproduct(second_in_bone,second_part_torque);
//break_force.add(vtemp);
//vtemp.crossproduct(first_in_bone,first_part_torque);
//break_force.sub(vtemp);
float bfm=break_force.magnitude()*ph_console::phBreakCommonFactor;
if(m_break_force<bfm)
{
second_part_force.mul(bfm/m_break_force);
m_pos_in_element.set(second_part_force);
//m_pos_in_element.add(break_force);
m_break_force=second_part_torque.x;
m_break_torque=second_part_torque.y;
m_add_torque_z=second_part_torque.z;
m_breaked=true;
#ifndef DBG_BREAK
return m_breaked;
#endif
}
#ifdef DBG_BREAK
Msg("bone_id %d break_torque - %f(max %f) break_force %f (max %f) breaked %d",m_bone_id,btm_dbg,m_break_torque,bfm,m_break_force,m_breaked);
#endif
return m_breaked;
}
