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());
//}
}
}
}
static dJointID find_shared_joint(dBodyID body1, dBodyID body2)
{
dJointID joint = 0;
if (body1 == 0)
{
if (body2 == 0)
return 0;
else
return find_shared_joint(body2, body1);
}
else
{
int i, n = dBodyGetNumJoints(body1);
for (i = 0; i < n; ++i)
if ((joint = dBodyGetJoint(body1, i)))
{
if (dJointGetBody(joint, 0) == body2 ||
dJointGetBody(joint, 1) == body2) return joint;
}
return 0;
}
}
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 BoxObstacle::remove() {
dGeomDestroy(boxGeom_);
if (box_ != 0) {
for(int i=0; i< dBodyGetNumJoints(box_); i++) {
dJointDestroy(dBodyGetJoint(box_, i));
}
dBodyDestroy(box_);
}
}
bool HasContact(dBodyID a)
{
if(a == 0) return false;
int n = dBodyGetNumJoints (a);
for(int i=0;i<n;i++) {
dJointID j=dBodyGetJoint (a,i);
if(dJointGetType(j)==dJointTypeContact) return true;
}
return false;
}
virtual void PhTune(dReal step)
{
InitValues();
int num=dBodyGetNumJoints(m_body);
for(int i=0;i<num;++i)
{
dJointID joint=dBodyGetJoint(m_body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointSetFeedback(joint,ContactFeedBacks.add());
}
}
}
bool HasContact(dBodyID a,dBodyID b)
{
if(a == 0 && b == 0) return false; //two environments
if(a == 0) Swap(a,b);
int n = dBodyGetNumJoints (a);
for(int i=0;i<n;i++) {
dJointID j=dBodyGetJoint (a,i);
if(dJointGetType(j)==dJointTypeContact) {
dBodyID j0=dJointGetBody(j,0);
dBodyID j1=dJointGetBody(j,1);
if(j0 == b || j1 == b) return true;
}
}
return false;
}
virtual void PhDataUpdate(dReal step)
{
int num=dBodyGetNumJoints(m_body);
for(int i=0;i<num;i++)
{
dJointID joint=dBodyGetJoint(m_body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointFeedback* feedback=dJointGetFeedback(joint);
R_ASSERT2(feedback,"Feedback was not set!!!");
dxJoint* b_joint=(dxJoint*) joint;
dBodyID other_body=b_joint->node[1].body;
bool b_body_second=(b_joint->node[1].body==m_body);
dReal* self_force=feedback->f1;
dReal* self_torque=feedback->t1;
dReal* othrers_force=feedback->f2;
dReal* othrers_torque=feedback->t2;
if(b_body_second)
{
other_body=b_joint->node[0].body;
self_force=feedback->f2;
self_torque=feedback->t2;
othrers_force=feedback->f1;
othrers_torque=feedback->t1;
}
save_max(m_max_force_self,_sqrt(dDOT( self_force,self_force)));
save_max(m_max_torque_self,_sqrt(dDOT( self_torque,self_torque)));
save_max(m_max_force_self_y,_abs(self_force[1]));
save_max(m_max_force_self_sd,_sqrt(self_force[0]*self_force[0]+self_force[2]*self_force[2]));
if(other_body)
{
dVector3 shoulder;dVectorSub(shoulder,dJointGetPositionContact(joint),dBodyGetPosition(other_body));
dReal shoulder_lenght=_sqrt(dDOT(shoulder,shoulder));
save_max(m_max_force_others,_sqrt(dDOT( othrers_force,othrers_force)));
if(!fis_zero(shoulder_lenght))
save_max(m_max_torque_others,_sqrt(dDOT( othrers_torque,othrers_torque))/shoulder_lenght);
}
}
}
}
static void drawGeom(dGeomID geomID)
{
// printf("1%lu", (uint64_t)geomID);
int gclass = dGeomGetClass(geomID);
// printf("2\n");
const dReal *pos = NULL;
const dReal *rot = NULL;
bool canDrawJoints = false;
ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID);
if (nullptr != userObj) {
dsSetColorAlpha(1, 1, 1, 1);
// printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]);
dsSetTexture (userObj->textureNum);
dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]);
} else {
dsSetColorAlpha(1, 1, 0, 1);
dsSetTexture (DS_WOOD);
}
switch (gclass) {
case dSphereClass:
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID));
}
canDrawJoints = true;
break;
case dBoxClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dVector3 lengths;
dGeomBoxGetLengths(geomID, lengths);
dsDrawBox(pos, rot, lengths);
}
canDrawJoints = true;
break;
}
case dCylinderClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dReal length;
dReal radius;
dGeomCylinderGetParams(geomID, &radius, &length);
dsDrawCylinder(pos, rot, length, radius);
}
canDrawJoints = true;
break;
}
default:
break;
}
// printf("class: %d\n", gclass);
if (canDrawJoints) {
#ifdef DRAW_JOINTS_TOO
dBodyID body = dGeomGetBody(geomID);
int numJoints = dBodyGetNumJoints(body);
for (int i = 0; i < numJoints; ++i)
{
dJointID joint = dBodyGetJoint(body, i);
int jointClass = dJointGetType(joint);
switch (jointClass)
{
case dJointTypeHinge:
{
dVector3 a11;
dBodyID body1 = dJointGetBody(joint, 0);
dBodyID body2 = dJointGetBody(joint, 1);
if (body1 && body2) {
const dReal* bodyPos1 = dBodyGetPosition(body1);
const dReal* bodyPos2 = dBodyGetPosition(body2);
dJointGetHingeAnchor(joint, a11);
dsSetColor(1, 0, 0);
dsDrawLine(a11, bodyPos1);
dsDrawLine(a11, bodyPos2);
dsSetColor(0, 1, 0);
dsDrawLine(bodyPos1, bodyPos2);
}
}
}
}
#endif
}
}
Int32 PhysicsBody::getNumJoints(void) const
{
return dBodyGetNumJoints(_BodyID);
}
/**
* This method updates the position and orientation of the grabbed
* object and all connected objects. If the grabbed object has
* no connections to other objects (via joints) the method simply
* applies the passed position and orientation to the grabbed object.
* Otherwise the calculation of the new transformation takes place in
* two steps:
* First the orientational difference between the grabbed object and
* and the passed orientation is calculated and applied to the object
* as torque. Then a simulation-step takes place, where the timestep
* is set to 0.5, what should leed to the half orientation correction.
* After the simulation step the objects velocities are set to zero.
* In the second step the difference of the objects position and the
* passed one are calculated and applied to the object as force. Like
* in step one a simulation-step with dt=0.5 takes place and the
* velocities are set to zero again.
* After the computation of the new transformations, the method iterates
* over all objects, which are grabbed or are linked with the grabbed
* object and copies their transformation from the ODE-representation
* to the Entity-members.
* At last the method calls the checkConstraints-method of all currently
* used joints, so that the joints can be activated or deactivated.
* @param position: position of the manipulating object
* @param orientation: orientation of the manipulating object
*/
TransformationData JointInteraction::update(TransformationData trans)
{
if (!isInitialized)
init();
gmtl::AxisAnglef AxAng;
gmtl::Vec3f diff;
dQuaternion quat;
userTrans.position = trans.position;
userTrans.orientation = trans.orientation;
if (!isGrabbing)
return identityTransformation();
int numJoints = dBodyGetNumJoints(grabbedObject->body);
if (!numJoints)
{ // Simple code for objects without Joints
convert(quat, userTrans.orientation);
dBodySetPosition(grabbedObject->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]);
dBodySetQuaternion(grabbedObject->body, quat);
}
else
{ // Code for object with Joints
TransformationData objectTrans;
gmtl::Quatf rotation;
gmtl::Vec3f force, torque;
const dReal* pos = dBodyGetPosition(grabbedObject->body);
const dReal* rot = dBodyGetQuaternion(grabbedObject->body);
convert(objectTrans.position, pos);
convert(objectTrans.orientation, rot);
// TODO: move this code to manipulationTerminationModel
// ungrab object if distance is too high
// diff = userTrans.position - objectTrans.position;
// if (gmtl::length(diff) > maxDist)
// {
// printf("JointInteraction::update(): distance %f too high!\n", gmtl::length(diff));
// ungrab();
// return identityTransformation();
// } // if
// STEP 1: apply Torque
// calculate Torque = angular offset from object orientation to current orientation
rotation = userTrans.orientation;
rotation *= invert(objectTrans.orientation);
gmtl::set(AxAng, rotation);
torque[0] = AxAng[0]*AxAng[1]; // rotAngle*rotX
torque[1] = AxAng[0]*AxAng[2]; // rotAngle*rotY
torque[2] = AxAng[0]*AxAng[3]; // rotAngle*rotZ
// TODO: move this code to manipulationTerminationModel
// ungrab object if angular distance is too high
// if (gmtl::length(torque) > maxRotDist)
// {
// printf("JointInteraction::update(): rotation %f too high!\n", gmtl::length(torque));
// ungrab();
// return identityTransformation();
// } // if
// create a Ball Joint to adjust orientation without changing the position
dJointID universalJoint = dJointCreateBall(world, 0);
dJointAttach(universalJoint, grabbedObject->body, 0);
dJointSetBallAnchor(universalJoint, pos[0], pos[1], pos[2]);
// printf("Torque = %f %f %f\n", torque[0], torque[1], torque[2]);
/***
* old way --> is very instable due to high timestep!!!
***
* dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
* // Half way to destination
* dWorldStep(world, 0.5);
**/
// take some simulation-steps for adjusting object's orientation
for (int i=0; i < stepsPerFrame; i++)
{
dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
dWorldStep(world, 1.0f/stepsPerFrame);
} // for
dJointDestroy(universalJoint);
// Reset speed and angular velocity to 0
dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
dBodySetAngularVel(grabbedObject->body, 0, 0, 0);
// STEP 2: apply force into wanted direction
// calculate Force = vector from object position to current position
force = (userTrans.position - objectTrans.position);
// printf("Force = %f %f %f\n", force[0], force[1], force[2]);
/***
* old way --> is very instable due to high timestep!!!
***
* dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
* // Half way to destination
* dWorldStep(world, 0.5);
**/
// take some simulation-steps for adjusting object's position
for (int i=0; i < stepsPerFrame; i++)
{
dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
dWorldStep(world, 1.0f/stepsPerFrame);
} // for
// Reset speed and angular velocity to 0
dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
dBodySetAngularVel(grabbedObject->body, 0, 0, 0);
}
// Get position and Orientation from simulated object
TransformationData newTransform = identityTransformation();
const dReal* pos = dBodyGetPosition(grabbedObject->body);
const dReal* rot = dBodyGetQuaternion(grabbedObject->body);
convert(newTransform.orientation, rot);
convert(newTransform.position, pos);
// Get position and Orientation from simulated objects
std::map<int, ODEObject*>::iterator it = linkedObjMap.begin();
ODEObject* object;
TransformationData linkedObjectTrans = identityTransformation();
while (it != linkedObjMap.end())
{
object = linkedObjMap[(*it).first];
if (object != grabbedObject)
{
linkedObjectTrans = object->entity->getWorldTransformation();
const dReal* pos1 = dBodyGetPosition(object->body);
const dReal* rot1 = dBodyGetQuaternion(object->body);
convert (linkedObjectTrans.orientation, rot1);
convert (linkedObjectTrans.position, pos1);
linkedObjPipes[it->first]->push_back(linkedObjectTrans);
// gmtl::set(AxAng, newRot);
/* object->entity->setTranslation(pos1[0], pos1[1], pos1[2]);
object->entity->setRotation(AxAng[1], AxAng[2], AxAng[3], AxAng[0]);*/
// assert(false);
// object->entity->setEnvironmentTransformation(linkedObjectTrans);
// object->entity->update();
} // if
++it;
} // while
// Update angles in HingeJoints and check joint-constraints
HingeJoint* hinge;
for (int i=0; i < (int)attachedJoints.size(); i++)
{
hinge = dynamic_cast<HingeJoint*>(attachedJoints[i]);
if (hinge)
hinge->checkAngles();
assert(attachedJoints[i]);
attachedJoints[i]->checkConstraints();
} // for
TransformationData result;
multiply(result, newTransform, deltaTrans);
TransformationPipe* pipe = linkedObjPipes[grabbedObject->entity->getEnvironmentBasedId()];
if (pipe)
pipe->push_back(result);
else
printd(WARNING, "JointInteraction::update(): Could not find Pipe for manipulated Object!\n");
return newTransform;
} // update
//#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;
}
void doStuff(const std::string & prefix,amarsi::Limbs limb){
std::ostringstream name;
name<<prefix<<"_TOE";
dBodyID toeBody=dWebotsGetBodyFromDEF(name.str().c_str());
dGeomID toe=dWebotsGetGeomFromDEF(name.str().c_str());
//std::cerr<<"Get Geom of value "<<toe<<std::endl;
if(!toe || !toeBody){
std::cerr<<"Did not found "<<name<<" "<<toe<<" "<<toeBody<<std::endl;
s_data->disablePhysics();
return;
}
std::ostringstream nameTibia;
nameTibia<<prefix<<"_FRONT_KNEE";
dBodyID tibia = dWebotsGetBodyFromDEF(nameTibia.str().c_str());
if(!tibia){
std::cerr<<"Did not found "<<nameTibia<<std::endl;
s_data->disablePhysics();
return;
}
// std::cerr<<nameTibia<<" "<<tibia<<std::endl;
int numJoint=dBodyGetNumJoints(tibia);
std::ostringstream nameFemur;
nameFemur<<prefix<<"_HIP_SERVO";
dBodyID femur = dWebotsGetBodyFromDEF(nameFemur.str().c_str());
if(!femur){
std::cerr<<"Did not found "<<nameFemur<<std::endl;
s_data->disablePhysics();
return;
}
for(int i=0;i<numJoint;++i){
dJointID j=dBodyGetJoint(tibia,i);
dBodyID b1= dJointGetBody(j,0);
dBodyID b2= dJointGetBody(j,1);
dReal pos[4];
if(b1==femur || b2==femur){
if(b1==femur)
dJointGetHingeAnchor2(j,pos);
else
dJointGetHingeAnchor(j,pos);
dVector3 posRel;
dBodyGetPosRelPoint(tibia,pos[0],pos[1],pos[3],posRel);
// std::cerr<<"pos : "<<posRel[0]<<" "<<posRel[1]<<" "<<posRel[2]<<std::endl;
// s_data->addKneeJoint(j,limb);
}
}
std::ostringstream nameAnkle;
nameAnkle<<prefix<<"_ANKLE"<<std::flush;
dBodyID ankleBody = dWebotsGetBodyFromDEF(nameAnkle.str().c_str());
dGeomID ankle=dWebotsGetGeomFromDEF(nameAnkle.str().c_str());
if(!ankle || !ankleBody){
std::cerr<<"Did not found "<<nameAnkle<<" "<<ankle<<" "
<<ankleBody<<std::endl;
s_data->disablePhysics();
return;
}
insertGeomInMonitored(ankle,limb);
insertGeomInMonitored(toe,limb);
s_data->addResettableBody(femur);
s_data->addResettableBody(tibia);
s_data->addResettableBody(ankleBody);
s_data->addResettableBody(toeBody);
}