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;
}
