Exemple #1
0
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]
			);
	}
}
Exemple #2
0
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);
  }
}
Exemple #3
0
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);
}
Exemple #6
0
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));
}
Exemple #7
0
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);
    }
}
Exemple #8
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);
  }
}
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;
}
Exemple #10
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);
    }
}
Exemple #11
0
//#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;
}