void ServoMotor::create(Joint* joint)
{
ASSERT(dJointGetType(joint->joint) == dJointTypeHinge || dJointGetType(joint->joint) == dJointTypeSlider);
this->joint = positionSensor.joint = joint;
if(dJointGetType(joint->joint) == dJointTypeHinge)
dJointSetHingeParam(joint->joint, dParamFMax, maxForce);
else
dJointSetSliderParam(joint->joint, dParamFMax, maxForce);
}
void osaODEServoMotor::SetVelocity( double qd ){
if( dJointGetType( MotorID() ) == dJointTypeAMotor ){
dJointSetAMotorParam( MotorID(), dParamVel, qd );
dJointSetAMotorParam( MotorID(), dParamFMax, ftmax );
}
if( dJointGetType( MotorID() ) == dJointTypeLMotor ){
dJointSetLMotorParam( MotorID(), dParamVel, qd/1000.0 );
dJointSetLMotorParam( MotorID(), dParamFMax, ftmax );
}
}
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());
//}
}
}
}
const dReal* dJointGetPositionContact(dJointID joint)
{
VERIFY2(dJointGetType(joint)==dJointTypeContact,"not a contact!");
dxJointContact* c_joint=(dxJointContact*)joint;
return c_joint->contact.geom.pos;
}
void SSimRobotEntity::setInitPosition(Vector3d pos)
{
int jsize = m_allJoints.size();
for (int i = 0; i < jsize; i++) {
SSimJoint *sjoint = m_allJoints[i];
SSimRobotParts rparts = sjoint->robotParts;
//dBodyID body = m_allJoints[i]->robotParts.objParts.body;
dBodyID body = rparts.objParts.body;
dJointID joint = sjoint->joint;
//dGeomID geom = rparts.objParts.geoms[0];
if (i == 0) {
dBodySetPosition(body, pos.x(), pos.y(), pos.z());
//dGeomSetPosition(geom,pos.x(),pos.y(),pos.z());
//const dReal *gpos = dGeomGetPosition(geom);
//LOG_MSG(("root body pos = (%f, %f, %f)", pos.x(), pos.y(), pos.z()));
//LOG_MSG(("root geom pos = (%f, %f, %f)", gpos[0], gpos[1], gpos[2]));
}
else {
// TODO: deal with a case in non-hinge joint
// gap between joint from root joint
dReal trans_x = sjoint->posFromRoot.x();
dReal trans_y = sjoint->posFromRoot.y();
dReal trans_z = sjoint->posFromRoot.z();
//LOG_MSG(("zure (%f, %f, %f)", trans_x, trans_y, trans_z));
//LOG_MSG(("zure of body(%f, %f, %f)", rparts.com.x(), rparts.com.y(), rparts.com.z()));
dBodySetPosition(body,
pos.x()+trans_x+rparts.com.x(),
pos.y()+trans_y+rparts.com.y(),
pos.z()+trans_z+rparts.com.z());
//const dReal *gpos = dGeomGetPosition(geom);
LOG_MSG(("body(id:%d) pos = (%f, %f, %f)",body, pos.x()+trans_x+rparts.com.x(), pos.y()+trans_y+rparts.com.y(), pos.z()+trans_z+rparts.com.z()));
//LOG_MSG(("geom pos = (%f, %f, %f)", gpos[0], gpos[1], gpos[2]));
// setting of joint position
int type = dJointGetType(joint);
if (type == dJointTypeHinge) {
dJointSetHingeAnchor(joint,
pos.x()+trans_x,
pos.y()+trans_y,
pos.z()+trans_z);
}
LOG_MSG(("joint(%d) pos = (%f, %f, %f)",joint, pos.x()+trans_x, pos.y()+trans_y, pos.z()+trans_z));
/*
dGeomSetPosition(geom,
pos.x()+trans_x+rparts.com.x(),
pos.y()+trans_y+rparts.com.y(),
pos.z()+trans_z+rparts.com.z());
*/
}
}
}
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;
}
static float get_phys_joint_value(dJointID j)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: return (float) DEG(dJointGetHingeAngle (j));
case dJointTypeSlider: return (float) dJointGetSliderPosition(j);
case dJointTypeHinge2: return (float) DEG(dJointGetHinge2Angle1 (j));
default: return 0.0f;
}
}
static void set_phys_joint_axis_2(dJointID j, const float v[3])
{
switch (dJointGetType(j))
{
case dJointTypeHinge2:
dJointSetHinge2Axis2 (j, v[0], v[1], v[2]); break;
case dJointTypeUniversal:
dJointSetUniversalAxis2(j, v[0], v[1], v[2]); break;
default: break;
}
}
static void set_phys_joint_attr(dJointID j, int p, float v)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: dJointSetHingeParam (j, p, v); break;
case dJointTypeSlider: dJointSetSliderParam (j, p, v); break;
case dJointTypeHinge2: dJointSetHinge2Param (j, p, v); break;
case dJointTypeUniversal: dJointSetUniversalParam(j, p, v); break;
default: break;
}
}
static float get_phys_joint_attr(dJointID j, int p)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: return (float) dJointGetHingeParam (j, p);
case dJointTypeSlider: return (float) dJointGetSliderParam (j, p);
case dJointTypeHinge2: return (float) dJointGetHinge2Param (j, p);
case dJointTypeUniversal: return (float) dJointGetUniversalParam(j, p);
default: return 0.0f;
}
}
void ServoMotor::registerObjects()
{
if(dJointGetType(joint->joint) == dJointTypeHinge)
positionSensor.unit = unit = "°";
else
positionSensor.unit = unit = "m";
positionSensor.fullName = joint->fullName + ".position";
fullName = joint->fullName + ".position";
CoreModule::application->registerObject(*CoreModule::module, positionSensor, joint);
CoreModule::application->registerObject(*CoreModule::module, *this, joint);
}
void ServoMotor::act()
{
const float currentPos = dJointGetType(joint->joint) == dJointTypeHinge
? dJointGetHingeAngle(joint->joint)
: dJointGetSliderPosition(joint->joint);
float setpoint = this->setpoint;
const float maxValueChange = maxVelocity * Simulation::simulation->scene->stepLength;
if(std::abs(setpoint - currentPos) > maxValueChange)
{
if(setpoint < currentPos)
setpoint = currentPos - maxValueChange;
else
setpoint = currentPos + maxValueChange;
}
const float newVel = controller.getOutput(currentPos, setpoint);
if(dJointGetType(joint->joint) == dJointTypeHinge)
dJointSetHingeParam(joint->joint, dParamVel, dReal(newVel));
else
dJointSetSliderParam(joint->joint, dParamVel, dReal(newVel));
}
/* returns the joint second anchoring point */
void joint_anchor2 (t_real *anchor, dJointID j) {
dVector3 joint_position;
int joint_type;
joint_type = dJointGetType (j);
if (joint_type == dJointTypeBall) {
dJointGetBallAnchor2 (j, joint_position);
}
else {
err_message ("Unrecognized joint type\n");
exit (EXIT_FAILURE);
}
vector_set (anchor, joint_position [0], joint_position [1], joint_position [2]);
}
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());
}
}
}
static float get_phys_joint_rate(dJointID j, int n)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: return (float) DEG(dJointGetHingeAngleRate (j));
case dJointTypeSlider: return (float) dJointGetSliderPositionRate(j);
case dJointTypeHinge2:
if (n == 1)
return (float) DEG(dJointGetHinge2Angle1Rate(j));
else
return (float) DEG(dJointGetHinge2Angle2Rate(j));
default: return 0.0f;
}
}
static void get_phys_joint_axis_2(dJointID j, float *v)
{
dVector3 V = { 0, 0, 0 };
switch (dJointGetType(j))
{
case dJointTypeHinge2: dJointGetHinge2Axis2 (j, V); break;
case dJointTypeUniversal: dJointGetUniversalAxis2(j, V); break;
default: break;
}
v[0] = (float) V[0];
v[1] = (float) V[1];
v[2] = (float) V[2];
}
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;
}
static void set_phys_joint_anchor(dJointID j, const float v[3])
{
switch (dJointGetType(j))
{
case dJointTypeBall:
dJointSetBallAnchor (j, v[0], v[1], v[2]); break;
case dJointTypeHinge:
dJointSetHingeAnchor (j, v[0], v[1], v[2]); break;
case dJointTypeHinge2:
dJointSetHinge2Anchor (j, v[0], v[1], v[2]); break;
case dJointTypeUniversal:
dJointSetUniversalAnchor(j, v[0], v[1], v[2]); break;
default: break;
}
}
void CCar::SWheel::Init()
{
if(inited) return;
BONE_P_PAIR_CIT bone=bone_map.find(bone_id);
R_ASSERT2(bone->second.element,"No Element was created for wheel. Check collision is set");
bone->second.element->set_DynamicLimits(default_l_limit,default_w_limit*100.f);
CPhysicsElement *e=bone->second.element ;
CPhysicsJoint *j=bone->second.joint ;
radius=e->getRadius();
R_ASSERT2(j,"No wheel joint was set for a wheel");
joint=j;
joint->SetBackRef(&joint);
R_ASSERT2(dJointGetType(joint->GetDJoint())==dJointTypeHinge2,"No wheel join was set for a wheel, only wheel-joint valid!!!");
ApplyDriveAxisVelTorque(0.f,0.f);
e->add_ObjectContactCallback(WheellCollisionCallback);
e->set_CallbackData((void*)&collision_params);
e->SetAirResistance(0,0);
inited=true;
}
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);
}
}
}
}
void VelocityMotor::registerObjects()
{
if(dJointGetType(joint->joint) == dJointTypeHinge)
{
positionSensor.unit = "?";
velocitySensor.unit = "?s";
}
else
{
positionSensor.unit = "m";
velocitySensor.unit = "m/s";
}
positionSensor.fullName = joint->fullName + ".position";
CoreModule::application->registerObject(*CoreModule::module, positionSensor, joint);
velocitySensor.fullName = joint->fullName + ".velocity";
CoreModule::application->registerObject(*CoreModule::module, velocitySensor, joint);
fullName = joint->fullName + ".velocity";
CoreModule::application->registerObject(*CoreModule::module, *this, joint);
}
Int32 PhysicsJoint::getJointType(void)
{
return dJointGetType(_JointID);
}
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
}
}
void SSimRobotEntity::addJoint(SSimJoint *joint)
{
// if a case that does not have geommetry
if (!joint->has_geom) {
// create dummy body
joint->robotParts.objParts.body = dBodyCreate(m_world);
dBodyDisable(joint->robotParts.objParts.body);
/*
dGeomID geom = dCreateSphere(m_space, 0.1);
dGeomSetBody(geom, joint->robotParts.objParts.body);
dGeomDisable(geom);
dMass mass;
dMassSetZero(&mass);
dMassSetSphereTotal(&mass, 0.0000001, 0.0000001);
dBodySetMass(joint->robotParts.objParts.body, &mass);
*/
//LOG_MSG(("dummy body %d",joint->robotParts.objParts.body));
}
// setting mass
else {
double mass = joint->robotParts.objParts.mass;
this->setMass(&(joint->robotParts.objParts), mass);
// add robot parts which have geometry to member variables
m_allParts.push_back(joint->robotParts);
}
// add joint to member variables
m_allJoints.push_back(joint);
// refer the joint ID and body ID
dJointID myJoint = joint->joint;
dBodyID cbody = joint->robotParts.objParts.body;
if (joint->isRoot) {
// connect with root body
//dJointAttach(myJoint, cbody, m_rootBody); //TODO!
return;
}
// refer the name of parent joint
std::string parent = joint->parent_joint;
// refer the parent joint structure from the joint name
//SSimJoint pjoint;
SSimJoint *pjoint = getJoint(parent);
//LOG_MSG(("parent %s %s",parent.c_str(), pjoint.name.c_str()));
// connect with parent body
dBodyID pbody = pjoint->robotParts.objParts.body;
//dJointAttach(myJoint, pbody, cbody);
dJointAttach(myJoint, cbody, pbody);
int type = dJointGetType(myJoint);
if (type == dJointTypeHinge) {
dJointSetHingeParam(myJoint, dParamLoStop, -2.0*M_PI);
dJointSetHingeParam(myJoint, dParamHiStop, 2.0*M_PI);
}
LOG_MSG(("joint(%d) attach body(%d, %d)",myJoint, cbody, pbody));
//dGeomID geom1 = pjoint->robotParts.objParts.mass;
//dGeomID geom2 = joint->robotParts.objParts.mass;
//LOG_MSG(("geom (%d, %d)", geom1, geom2));
/*
// if geommetry exists
if (joint.has_geom) {
/*
////// search the parent part to be connected
// refer the parent joint
std::string parent = joint.parent_joint;
while (1) {
// get joint structure from joint name
SSimJoint pjoint = getJoint(parent);
// check whether geometry to be connected exist?
if (pjoint.has_geom) {
// refer a body to be connected
dBodyID pbody = pjoint.robotParts.objParts.body;
dJointAttach(myJoint, cbody, pbody);
break;
}
// there is no parent any more
if (pjoint.isRoot) break;
else{
// Search the parents in addition
parent = pjoint.parent_name;
}
}
*/
}
void VelocityMotor::VelocitySensor::updateValue()
{
data.floatValue = dJointGetType(joint->joint) == dJointTypeHinge
? dJointGetHingeParam(joint->joint, dParamVel)
: dJointGetSliderParam(joint->joint, dParamVel);
}
void ServoMotor::PositionSensor::updateValue()
{
data.floatValue = dJointGetType(joint->joint) == dJointTypeHinge
? dJointGetHingeAngle(joint->joint)
: dJointGetSliderPosition(joint->joint);
}
//#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;
}
