void BodyCutForce(dBodyID body,float l_limit,float w_limit) { const dReal wa_limit=w_limit/fixed_step; const dReal* force= dBodyGetForce(body); dReal force_mag=dSqrt(dDOT(force,force)); //body mass dMass m; dBodyGetMass(body,&m); dReal force_limit =l_limit/fixed_step*m.mass; if(force_mag>force_limit) { dBodySetForce(body, force[0]/force_mag*force_limit, force[1]/force_mag*force_limit, force[2]/force_mag*force_limit ); } const dReal* torque=dBodyGetTorque(body); dReal torque_mag=dSqrt(dDOT(torque,torque)); if(torque_mag<0.001f) return; dMatrix3 tmp,invI,I; // compute inertia tensor in global frame dMULTIPLY2_333 (tmp,m.I,body->R); dMULTIPLY0_333 (I,body->R,tmp); // compute inverse inertia tensor in global frame dMULTIPLY2_333 (tmp,body->invI,body->R); dMULTIPLY0_333 (invI,body->R,tmp); //angular accel dVector3 wa; dMULTIPLY0_331(wa,invI,torque); dReal wa_mag=dSqrt(dDOT(wa,wa)); if(wa_mag>wa_limit) { //scale w for(int i=0;i<3;++i)wa[i]*=wa_limit/wa_mag; dVector3 new_torqu; dMULTIPLY0_331(new_torqu,I,wa); dBodySetTorque ( body, new_torqu[0], new_torqu[1], new_torqu[2] ); } }
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 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 dxGeomTransform::computeFinalTx() { dMULTIPLY0_331 (final_pos,R,obj->pos); final_pos[0] += pos[0]; final_pos[1] += pos[1]; final_pos[2] += pos[2]; dMULTIPLY0_333 (final_R,R,obj->R); }
void dxGeomTransform::computeFinalTx() { dMULTIPLY0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos); transform_posr.pos[0] += final_posr->pos[0]; transform_posr.pos[1] += final_posr->pos[1]; transform_posr.pos[2] += final_posr->pos[2]; dMULTIPLY0_333 (transform_posr.R,final_posr->R,obj->final_posr->R); }
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)); }
void dRigidBodyArraySetRotationRelative(dRigidBodyArrayID bodyArray, const dReal *Rs) { dBodyID center = bodyArray->center; const dReal *p0 = dBodyGetPosition(center); const dReal *R0 = dBodyGetRotation(center); dMatrix3 R0Rs; dMULTIPLY0_333(R0Rs, R0, Rs); dMatrix3 R0RsR0t; dMULTIPLY2_333(R0RsR0t, R0Rs, R0); for(size_t i = 0; i < dRigidBodyArraySize(bodyArray); i++) { dBodyID body = dRigidBodyArrayGet(bodyArray, i); const dReal *pi = dBodyGetPosition(body); const dReal *Ri = dBodyGetRotation(body); dMatrix3 R1; dMULTIPLY0_333(R1, R0RsR0t, Ri); dVector3 p1, pi_p0, R0RsR0t__pi_p0; dOP(pi_p0, -, pi, p0); dMULTIPLY0_331(R0RsR0t__pi_p0, R0RsR0t, pi_p0); dOP(p1, +, R0RsR0t__pi_p0, p0); dBodySetPosition(body, p1[0], p1[1], p1[2]); dBodySetRotation(body, R1); } }
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); } }
void dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations) { dxBody *bodyPair[2], *body; dReal *GIPair[2], *GinvIPair[2]; dxJoint *joint; int iter, b, j, i; dReal ministep = stepsize / maxiterations; // make a local copy of the joint array, because we might want to modify it. // (the "dxJoint *const*" declaration says we're allowed to modify the joints // but not the joint array, because the caller might need it unchanged). dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); memcpy (joints, _joints, nj * sizeof (dxJoint *)); // get m = total constraint dimension, nub = number of unbounded variables. // create constraint offset array and number-of-rows array for all joints. // the constraints are re-ordered as follows: the purely unbounded // constraints, the mixed unbounded + LCP constraints, and last the purely // LCP constraints. this assists the LCP solver to put all unbounded // variables at the start for a quick factorization. // // joints with m=0 are inactive and are removed from the joints array // entirely, so that the code that follows does not consider them. // also number all active joints in the joint list (set their tag values). // inactive joints receive a tag value of -1. int m = 0; dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); int *ofs = (int *) ALLOCA (nj * sizeof (int)); for (i = 0, j = 0; j < nj; j++) { // i=dest, j=src joints[j]->vtable->getInfo1 (joints[j], info + i); if (info[i].m > 0) { joints[i] = joints[j]; joints[i]->tag = i; i++; } else { joints[j]->tag = -1; } } nj = i; // the purely unbounded constraints for (i = 0; i < nj; i++) { ofs[i] = m; m += info[i].m; } dReal *c = NULL; dReal *cfm = NULL; dReal *lo = NULL; dReal *hi = NULL; int *findex = NULL; dReal *J = NULL; dxJoint::Info2 * Jinfo = NULL; if (m) { // create a constraint equation right hand side vector `c', a constraint // force mixing vector `cfm', and LCP low and high bound vectors, and an // 'findex' vector. c = (dReal *) ALLOCA (m * sizeof (dReal)); cfm = (dReal *) ALLOCA (m * sizeof (dReal)); lo = (dReal *) ALLOCA (m * sizeof (dReal)); hi = (dReal *) ALLOCA (m * sizeof (dReal)); findex = (int *) ALLOCA (m * sizeof (int)); dSetZero (c, m); dSetValue (cfm, m, world->global_cfm); dSetValue (lo, m, -dInfinity); dSetValue (hi, m, dInfinity); for (i = 0; i < m; i++) findex[i] = -1; // get jacobian data from constraints. a (2*m)x8 matrix will be created // to store the two jacobian blocks from each constraint. it has this // format: // // l l l 0 a a a 0 \ . // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) // l l l 0 a a a 0 / // l l l 0 a a a 0 \ . // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) // l l l 0 a a a 0 / // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) // etc... // // (lll) = linear jacobian data // (aaa) = angular jacobian data // J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); dSetZero (J, 2 * m * 8); Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); for (i = 0; i < nj; i++) { Jinfo[i].rowskip = 8; Jinfo[i].fps = dRecip (stepsize); Jinfo[i].erp = world->global_erp; Jinfo[i].J1l = J + 2 * 8 * ofs[i]; Jinfo[i].J1a = Jinfo[i].J1l + 4; Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; Jinfo[i].J2a = Jinfo[i].J2l + 4; Jinfo[i].c = c + ofs[i]; Jinfo[i].cfm = cfm + ofs[i]; Jinfo[i].lo = lo + ofs[i]; Jinfo[i].hi = hi + ofs[i]; Jinfo[i].findex = findex + ofs[i]; //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); } } dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); for (b = 0; b < nb; b++) { for (i = 0; i < 4; i++) { saveFacc[b * 4 + i] = bodies[b]->facc[i]; saveTacc[b * 4 + i] = bodies[b]->tacc[i]; } bodies[b]->tag = b; } for (iter = 0; iter < maxiterations; iter++) { dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; for (b = 0; b < nb; b++) { body = bodies[b]; // for all bodies, compute the inertia tensor and its inverse in the global // frame, and compute the rotational force and add it to the torque // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. // @@@ check computation of rotational force. // compute inertia tensor in global frame dMULTIPLY2_333 (tmp, body->mass.I, body->posr.R); dMULTIPLY0_333 (globalI + b * 12, body->posr.R, tmp); // compute inverse inertia tensor in global frame dMULTIPLY2_333 (tmp, body->invI, body->posr.R); dMULTIPLY0_333 (globalInvI + b * 12, body->posr.R, tmp); for (i = 0; i < 4; i++) body->tacc[i] = saveTacc[b * 4 + i]; // add the gravity force to all bodies if ((body->flags & dxBodyNoGravity) == 0) { body->facc[0] = saveFacc[b * 4 + 0] + dMUL(body->mass.mass,world->gravity[0]); body->facc[1] = saveFacc[b * 4 + 1] + dMUL(body->mass.mass,world->gravity[1]); body->facc[2] = saveFacc[b * 4 + 2] + dMUL(body->mass.mass,world->gravity[2]); body->facc[3] = 0; } else { body->facc[0] = saveFacc[b * 4 + 0]; body->facc[1] = saveFacc[b * 4 + 1]; body->facc[2] = saveFacc[b * 4 + 2]; body->facc[3] = 0; } } #ifdef RANDOM_JOINT_ORDER //randomize the order of the joints by looping through the array //and swapping the current joint pointer with a random one before it. for (j = 0; j < nj; j++) { joint = joints[j]; dxJoint::Info1 i1 = info[j]; dxJoint::Info2 i2 = Jinfo[j]; const int r = dRandInt(j+1); joints[j] = joints[r]; info[j] = info[r]; Jinfo[j] = Jinfo[r]; joints[r] = joint; info[r] = i1; Jinfo[r] = i2; } #endif //now iterate through the random ordered joint array we created. for (j = 0; j < nj; j++) { joint = joints[j]; bodyPair[0] = joint->node[0].body; bodyPair[1] = joint->node[1].body; if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled)) bodyPair[0] = 0; if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled)) bodyPair[1] = 0; //if this joint is not connected to any enabled bodies, skip it. if (!bodyPair[0] && !bodyPair[1]) continue; if (bodyPair[0]) { GIPair[0] = globalI + bodyPair[0]->tag * 12; GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12; } if (bodyPair[1]) { GIPair[1] = globalI + bodyPair[1]->tag * 12; GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12; } joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); //dInternalStepIslandFast is an exact copy of the old routine with one //modification: the calculated forces are added back to the facc and tacc //vectors instead of applying them to the bodies and moving them. if (info[j].m > 0) { dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep); } } // } //Now we can simulate all the free floating bodies, and move them. for (b = 0; b < nb; b++) { body = bodies[b]; for (i = 0; i < 4; i++) { body->facc[i] = dMUL(body->facc[i],ministep); body->tacc[i] = dMUL(body->tacc[i],ministep); } //apply torque dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc); //apply force for (i = 0; i < 3; i++) body->lvel[i] += dMUL(body->invMass,body->facc[i]); //move It! moveAndRotateBody (body, ministep); } } for (b = 0; b < nb; b++) for (j = 0; j < 4; j++) bodies[b]->facc[j] = bodies[b]->tacc[j] = 0; }
// 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; }