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; }