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 dRigidBodyArraySetRotation(dRigidBodyArrayID bodyArray, const dReal *Rs) {
dBodyID center = bodyArray->center;
const dReal *p0 = dBodyGetPosition(center);
const dReal *R0 = dBodyGetRotation(center);
dMatrix3 RsR0t;
dMULTIPLY2_333(RsR0t, Rs, 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, RsR0t, Ri);
dVector3 p1, pi_p0, R0RsR0t__pi_p0;
dOP(pi_p0, -, pi, p0);
dMULTIPLY0_331(R0RsR0t__pi_p0, RsR0t, pi_p0);
dOP(p1, +, R0RsR0t__pi_p0, p0);
dBodySetPosition(body, p1[0], p1[1], p1[2]);
dBodySetRotation(body, R1);
}
}
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;
}
//#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;
}
