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