void DL_pts::get_force_info(int i, DL_actuator_type& at,
DL_dyna*& _g, DL_point *_p, DL_vector *_v){
if (i==0) {
if (g_is_dyna) {
at=force;
_g=(DL_dyna*)g;
_p->assign(&p);
reactionforce(_v);
return;
}
if (sg_is_dyna) {
at=force;
_g=(DL_dyna*)(surf->get_geo());
_p->assign(&sstl);
reactionforce(_v);
_v->neg(_v);
return;
}
}
if ((i==1) && g_is_dyna && sg_is_dyna) {
at=force;
_g=(DL_dyna*)(surf->get_geo());
_p->assign(&sstl);
reactionforce(_v);
_v->neg(_v);
return;
}
at=none;
};
void DL_vtv::get_force_info(int i, DL_actuator_type& at,
DL_dyna*& _g, DL_point *_p, DL_vector *_v) {
if (i==0) {
at=force;
_g=d;
_p->assign(&pd);
reactionforce(_v);
return;
}
if ((i==1) && (g_is_dyna)) {
at=force;
_g=(DL_dyna*)g;
_p->assign(&pg);
reactionforce(_v);
_v->neg(_v);
return;
}
at=none;
};
void DL_wheel::get_force_info(int i, DL_actuator_type& at,
DL_dyna*& _g, DL_point *_p, DL_vector *_v){
if (i==0) {
at=force;
_g=w;
_p->assign(&wpc);
reactionforce(_v);
return;
}
if ((i==1) && (sg_is_dyna)) {
at=force;
_g=(DL_dyna*)surf->get_geo();
_p->assign(&spc);
reactionforce(_v);
_v->neg(_v);
return;
}
at=none;
};
dVector ForceBetweenBody (NewtonBody* const body0, NewtonBody* const body1)
{
dVector reactionforce (0.0f);
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body0); joint; joint = NewtonBodyGetNextContactJoint(body0, joint)) {
if (NewtonJointIsActive(joint) && (NewtonJointGetBody0(joint) == body0) || (NewtonJointGetBody0(joint) == body1)) {
for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) {
dVector point(0.0f);
dVector normal(0.0f);
dVector contactForce(0.0f);
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactPositionAndNormal (material, body0, &point.m_x, &normal.m_x);
NewtonMaterialGetContactForce(material, body0, &contactForce[0]);
reactionforce += contactForce;
}
break;
}
}
return reactionforce;
}
dFloat ForceBodyAccelerationMichio (NewtonBody* const body)
{
dVector reactionforce (0.0f);
// calcualte accelration generate by all contacts
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
if (NewtonJointIsActive(joint)) {
for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) {
dVector contactForce(0.0f);
NewtonMaterial* const material = NewtonContactGetMaterial(contact);
NewtonMaterialGetContactForce(material, body, &contactForce[0]);
reactionforce += contactForce;
}
}
}
dMatrix matrix;
dVector accel;
dVector veloc;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodyGetAcceleration(body, &accel[0]);
accel -= reactionforce.Scale (1.0f/mass);
//calculate centripetal acceleration here.
NewtonBodyGetMatrix(body, &matrix[0][0]);
dVector radius(matrix.m_posit.Scale(-1.0f));
radius.m_w = 0.0f;
dFloat radiusMag = dSqrt(radius.DotProduct3(radius));
dVector radiusDir (radius.Normalize());
NewtonBodyGetVelocity(body, &veloc[0]);
veloc += radiusDir.Scale(veloc.DotProduct3(radiusDir));
dVector centripetalAccel(veloc.DotProduct3(veloc) / radiusMag);
accel += centripetalAccel;
return dSqrt (accel.DotProduct3(accel));
}
