void drawGeom (dGeomID g, const dReal *pos, const dReal *R) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCCylinderClass) { dReal radius,length; dGeomCCylinderGetParams (g,&radius,&length); dsDrawCappedCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R); } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!draw_geom){ return; } if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } /* // cylinder option not yet implemented else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } */ else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
void TSRODERigidBody::DebugRender() { Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState ); TSRMatrix4 bodyTransform; TSRMatrix4 geomTransform; const float* pBodyPosition = dBodyGetPosition( m_BodyID ); const float* pBodyRotation = dBodyGetRotation( m_BodyID ); ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation ); TSRGlobalConstants.PushMatrix(); TSRGlobalConstants.MultMatrix( bodyTransform.d ); TSRDebugDraw::RenderAxis( 1.0f ); TSRDebugDraw::RenderSphere( 0.25f ); for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ ) { dGeomID currGeomTransformID = m_GeomIDs[ i ]; dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID ); const float* pGeomPosition = dGeomGetPosition( geomID ); const float* pGeomRotation = dGeomGetRotation( geomID ); ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation ); TSRGlobalConstants.PushMatrix(); TSRGlobalConstants.MultMatrix( bodyTransform.d ); switch( dGeomGetClass( geomID ) ) { case dBoxClass: { dVector3 extents; dGeomBoxGetLengths( geomID, extents ); TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] ); TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] ); aabbMin *= 0.5f; aabbMax *= 0.5f; TSRDebugDraw::RenderAABB( aabbMin, aabbMax ); } break; case dSphereClass: { float radius; radius = dGeomSphereGetRadius( geomID ); TSRDebugDraw::RenderSphere( radius ); } break; case dCylinderClass: { float radius,length; dGeomCylinderGetParams( geomID, &radius, &length ); TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) ); } break; } TSRGlobalConstants.PopMatrix(); } TSRGlobalConstants.PopMatrix(); Graphics()->SetRasterizerState( Graphics()->m_FillSolidState ); }
/******************************************************************************* Function to draw a geometry object. *******************************************************************************/ void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const { if( !g ) //If the geometry object is missing, end the function. return; if( !position ) //Position was not passed? position = dGeomGetPosition( g ); //Then, get the geometry position. if( !orientation ) //Orientation was not given? orientation = dGeomGetRotation( g ); //And get existing geometry orientation. int type = dGeomGetClass( g ); //Get the type of geometry. if( type == dBoxClass ) //Is it a box? { dReal sides[3]; dGeomBoxGetLengths( g, sides ); //Get length of sides. renderBox( sides, position, orientation ); //Render the actual box in environment. } if( type == dSphereClass ) //Is it a sphere? { dReal radius; radius = dGeomSphereGetRadius( g ); //Get the radius. renderSphere( radius, position, orientation ); //Render sphere in environment. } if( type == dCapsuleClass ) { dReal radius; dReal length; dGeomCapsuleGetParams( g, &radius, &length ); //Get both radius and length. renderCapsule( radius, length, position, orientation ); //Render capsule in environment. } if( type == dGeomTransformClass ) //Is it an embeded geom in a composite body. { dGeomID g2 = dGeomTransformGetGeom( g ); //Get the actual geometry inside the wrapper. const dReal *position2 = dGeomGetPosition( g2 ); //Get position and orientation of wrapped geometry. const dReal *orientation2 = dGeomGetRotation( g2 ); dVector3 actualPosition; //Real world coordinated position and orientation dMatrix3 actualOrientation; //of the wrapped geometry. dMultiply0_331( actualPosition, orientation, position2 ); //Get world coordinates of geometry position. actualPosition[0] += position[0]; actualPosition[1] += position[1]; actualPosition[2] += position[2]; dMultiply0_333( actualOrientation, orientation, orientation2 ); //Get world coordinates of geom orientation. drawGeom( g2, actualPosition, actualOrientation ); //Draw embeded geometry. } }
static void computeFinalTx(dGeomID geom_transform,dReal* final_pos,dReal* final_R) { R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform"); dGeomID obj=dGeomTransformGetGeom(geom_transform); const dReal *R =dGeomGetRotation(geom_transform); const dReal *pos=dGeomGetPosition(geom_transform); dMULTIPLY0_331 (final_pos,R,dGeomGetPosition(obj)); final_pos[0] += pos[0]; final_pos[1] += pos[1]; final_pos[2] += pos[2]; dMULTIPLY0_333 (final_R,R,dGeomGetRotation(obj)); }
static void printGeomTransform (PrintingContext &c, dxGeom *g) { dxGeom *g2 = dGeomTransformGetGeom (g); const dReal *pos = dGeomGetPosition (g2); dQuaternion q; dGeomGetQuaternion (g2,q); c.print ("type","transform"); c.print ("pos",pos); c.print ("q",q,4); c.print ("geometry = {"); c.indent++; printGeom (c,g2); c.indent--; c.print ("}"); }
// simulation loop static void simLoop (int pause) { static bool todo = false; if ( todo ) { // DEBUG static int cnt = 0; ++cnt; if (cnt == 5) command ( 'q' ); if (cnt == 10) dsStop(); } if (!pause) { double simstep = 0.01; // 10ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf (dt/simstep); if (!nrofsteps) nrofsteps = 1; for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldStep (world, simstep); dJointGroupEmpty (contactgroup); } update(); dReal radius, length; dsSetTexture (DS_WOOD); drawBox (geom[W], 1,1,0); drawBox (geom[EXT], 0,1,0); dVector3 anchorPos; dReal ang1 = 0; dReal ang2 = 0; dVector3 axisP, axisR1, axisR2; if ( dJointTypePU == type ) { dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); ang1 = pu->getAngle1(); ang2 = pu->getAngle2(); pu->getAxis1 (axisR1); pu->getAxis2 (axisR2); pu->getAxisP (axisP); dJointGetPUAnchor (pu->id(), anchorPos); } else if ( dJointTypePR == type ) { dPRJoint *pr = dynamic_cast<dPRJoint *> (joint); pr->getAxis1 (axisP); pr->getAxis2 (axisR1); dJointGetPRAnchor (pr->id(), anchorPos); } // Draw the axisR if ( geom[INT] ) { dsSetColor (1,0,1); dVector3 l; dGeomBoxGetLengths (geom[INT], l); const dReal *rotBox = dGeomGetRotation (geom[W]); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i]; dsDrawBox (pos, rotBox, l); } dsSetTexture (DS_CHECKERED); if ( geom[AXIS1] ) { dQuaternion q, qAng; dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dGeomGetQuaternion (geom[AXIS1], q); dQuaternion qq; dQMultiply1 (qq, qAng, q); dMatrix3 R; dQtoR (qq,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length); dsSetColor (1,0,0); dsDrawCylinder (anchorPos, R, length, radius); } if ( dJointTypePU == type && geom[AXIS2] ) { //dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); dQuaternion q, qAng, qq, qq1; dGeomGetQuaternion (geom[AXIS2], q); dQFromAxisAndAngle (qAng, 0, 1, 0, ang2); dQMultiply1 (qq, qAng, q); dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dQMultiply1 (qq1, qAng, qq); dMatrix3 R; dQtoR (qq1,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length); dsSetColor (0,0,1); dsDrawCylinder (anchorPos, R, length, radius); } dsSetTexture (DS_WOOD); // Draw the anchor if ( geom[ANCHOR] ) { dsSetColor (1,1,1); dVector3 l; dGeomBoxGetLengths (geom[ANCHOR], l); const dReal *rotBox = dGeomGetRotation (geom[D]); const dReal *posBox = dGeomGetPosition (geom[D]); dVector3 e; for (int i=0; i<3; ++i) e[i] = posBox[i] - anchorPos[i]; dNormalize3 (e); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i]; dsDrawBox (pos, rotBox, l); } drawBox (geom[D], 1,1,0); } }
//! add ODE parts with fixed joint void SSimEntity::setMass(double mass) { m_parts.mass = mass; int geomNum = getGeomNum(); if (geomNum != 0) { // if a part is already added // mass per part double ms = mass / geomNum; // refer geometry and body dBodyID body = m_parts.body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // refer the type of geometory dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } const dReal *pos = dGeomGetPosition(geom); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } /* for (int i = 0; i < geomNum; i++) { dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); const dReal *pos = dGeomGetPosition(geom); // Change to a relative position from CoG dGeomSetPosition(geom, pos[0] - m.c[0], pos[1] - m.c[1], pos[2] - m.c[2]); } // keep the CoG position //m_parts.pos.set(m.c[0], m.c[1], m.c[2]); */ // Adjustment of the gap from CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // Setting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
//! setting of mass void SSimRobotEntity::setMass(SSimObjParts *parts, double mass) { // if a part is already added int geomNum = parts->geoms.size(); if (geomNum != 0) { // mass per each part double ms = mass / geomNum; // refer geometry and body dBodyID body = parts->body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // Refer the type of geometry dGeomID geom = dGeomTransformGetGeom(parts->geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } // const dReal *pos = dGeomGetPosition(geom); //LOG_MSG(("pos = (%f, %f, %f)", pos[0], pos[1], pos[2])); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } // adjustment of the gap between CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // seeting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } //<---- Convex Object else if (type == dConvexClass) { #if 0 dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); #else dsDrawConvex(pos,R, Sphere_planes, Sphere_planecount, Sphere_points, Sphere_pointcount, Sphere_polygons); #endif } //----> Convex Object else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_body) { dBodyID body = dGeomGetBody(g); if (body) { const dReal *bodypos = dBodyGetPosition (body); const dReal *bodyr = dBodyGetRotation (body); dReal bodySides[3] = { 0.1, 0.1, 0.1 }; dsSetColorAlpha(0,1,0,1); dsDrawBox(bodypos,bodyr,bodySides); } } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
// copied from an OpenDE demo program void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (g == nullptr) return; if (dGeomIsSpace(g) != 0) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } else show_aabb = 0; if (show_aabb != 0) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
//#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; }