dReal ServoMotor::getTorque() {
// code from Jeff Shim
osg::Vec3 torque1(fback_.t1[0], fback_.t1[1], fback_.t1[2] );
osg::Vec3 torque2(fback_.t2[0], fback_.t2[1], fback_.t2[2] );
osg::Vec3 force1(fback_.f1[0], fback_.f1[1], fback_.f1[2] );
osg::Vec3 force2(fback_.f2[0], fback_.f2[1], fback_.f2[2] );
const double* p1 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),0) );
const double* p2 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),1) );
osg::Vec3 pos1(p1[0], p1[1], p1[2]);
osg::Vec3 pos2(p2[0], p2[1], p2[2]);
dVector3 odeAnchor;
dJointGetHingeAnchor ( joint_->getJoint(), odeAnchor );
osg::Vec3 anchor(odeAnchor[0], odeAnchor[1], odeAnchor[2]);
osg::Vec3 ftorque1 = torque1 - (force1^(pos1-anchor));// torq by motor = total torq - constraint torq
osg::Vec3 ftorque2 = torque2 - (force2^(pos2-anchor));// opposite direction - use if this is necessary
dVector3 odeAxis;
dJointGetHingeAxis ( joint_->getJoint(), odeAxis);
osg::Vec3 axis(odeAxis[0], odeAxis[1], odeAxis[2] );
axis.normalize();
double torque = ftorque1 * axis;
//printf ("torque: % 1.10f\n", torque);
return torque;
}
/*** コールバック関数 ***/
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
dVector3 tmp_fdir = {0, 0, 0, 0};
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return;
int wheel_flag = 0;
for (int j = 0; j < WHEEL_NUM; j++)
{
if ((o1 == wheel[j].geom)||(o2 == wheel[j].geom))
{
wheel_flag = 1;
dJointGetHingeAxis(wheel[j].joint,tmp_fdir);
break;
}
}
static const int N = 10;
dContact contact[N];
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0)
{
if (wheel_flag == 1)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactFDir1| dContactMu2 | dContactSoftERP | dContactSoftCFM;
contact[i].fdir1[0] = tmp_fdir[0]; // 第１摩擦方向の設定(x軸成分)
contact[i].fdir1[1] = tmp_fdir[1]; // 第１摩擦方向の設定(y軸成分)
contact[i].fdir1[2] = tmp_fdir[2]; // 第１摩擦方向の設定(z軸成分)
contact[i].surface.mu = 0.1; // 車軸方向の摩擦係数
contact[i].surface.mu2 = dInfinity; // 車輪方向の摩擦係数
contact[i].surface.soft_erp = 0.9;
contact[i].surface.soft_cfm = 0.001;
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
dJointAttach(c,b1,b2);
}
}
else
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.soft_erp = 0.8;
contact[i].surface.soft_cfm = 1e-5;
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
dJointAttach(c,b1,b2);
}
}
}
}
void
dxJointHinge::setRelativeValues()
{
dVector3 vec;
dJointGetHingeAnchor(this, vec);
setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 );
dJointGetHingeAxis(this, vec);
setAxes( this, vec[0], vec[1], vec[2], axis1, axis2 );
computeInitialRelativeRotation();
}
static void get_phys_joint_axis_1(dJointID j, float *v)
{
dVector3 V = { 0, 0, 0 };
switch (dJointGetType(j))
{
case dJointTypeHinge: dJointGetHingeAxis (j, V); break;
case dJointTypeSlider: dJointGetSliderAxis (j, V); break;
case dJointTypeHinge2: dJointGetHinge2Axis1 (j, V); break;
case dJointTypeUniversal: dJointGetUniversalAxis1(j, V); break;
default: break;
}
v[0] = (float) V[0];
v[1] = (float) V[1];
v[2] = (float) V[2];
}
/**
*@brief 位置更新の関数
*/
void ODEJointObj::UpdateJointPosition()
{
ms->mu->lock();
if(JointType == 0)
{
dVector3 P;
dJointGetSliderAxis(joint, P);
}
else if(JointType == 2)
{
dVector3 P;
dJointGetHingeAnchor(joint, P);
SetPosition(bscale_x*P[0] + offx, bscale_y*P[1] + offy, bscale_z*P[2] + offz);
dJointGetHingeAxis(joint, P);
}
ms->mu->unlock();
}
soyatomsAxis* soy_joints_hinge_get_axis (soyjointsHinge* self) {
soyatomsAxis* result;
dxVector3* _tmp0_;
dxVector3* v;
struct dxJoint* _tmp1_;
dxVector3* _tmp2_;
soyatomsAxis* _tmp3_;
soyatomsAxis* _tmp4_;
soyatomsAxis* value;
gboolean _tmp5_ = FALSE;
gboolean _tmp6_ = FALSE;
gboolean _tmp7_ = FALSE;
soyatomsAxis* _tmp8_;
gboolean _tmp14_;
gboolean _tmp20_;
gboolean _tmp26_;
g_return_val_if_fail (self != NULL, NULL);
_tmp0_ = dvector3_new ();
v = _tmp0_;
_tmp1_ = ((soyjointsJoint*) self)->joint;
_tmp2_ = v;
dJointGetHingeAxis ((struct dxJoint*) _tmp1_, _tmp2_);
_tmp3_ = self->priv->_axis_obj;
_tmp4_ = _g_object_ref0 (_tmp3_);
value = _tmp4_;
_tmp8_ = value;
if (_tmp8_ == NULL) {
_tmp7_ = TRUE;
} else {
dxVector3* _tmp9_;
dReal _tmp10_;
soyatomsAxis* _tmp11_;
gfloat _tmp12_;
gfloat _tmp13_;
_tmp9_ = v;
_tmp10_ = _tmp9_->x;
_tmp11_ = value;
_tmp12_ = soy_atoms_axis_get_x (_tmp11_);
_tmp13_ = _tmp12_;
_tmp7_ = ((gfloat) _tmp10_) != _tmp13_;
}
_tmp14_ = _tmp7_;
if (_tmp14_) {
_tmp6_ = TRUE;
} else {
dxVector3* _tmp15_;
dReal _tmp16_;
soyatomsAxis* _tmp17_;
gfloat _tmp18_;
gfloat _tmp19_;
_tmp15_ = v;
_tmp16_ = _tmp15_->y;
_tmp17_ = value;
_tmp18_ = soy_atoms_axis_get_y (_tmp17_);
_tmp19_ = _tmp18_;
_tmp6_ = ((gfloat) _tmp16_) != _tmp19_;
}
_tmp20_ = _tmp6_;
if (_tmp20_) {
_tmp5_ = TRUE;
} else {
dxVector3* _tmp21_;
dReal _tmp22_;
soyatomsAxis* _tmp23_;
gfloat _tmp24_;
gfloat _tmp25_;
_tmp21_ = v;
_tmp22_ = _tmp21_->z;
_tmp23_ = value;
_tmp24_ = soy_atoms_axis_get_z (_tmp23_);
_tmp25_ = _tmp24_;
_tmp5_ = ((gfloat) _tmp22_) != _tmp25_;
}
_tmp26_ = _tmp5_;
if (_tmp26_) {
soyatomsAxis* _tmp27_;
dxVector3* _tmp31_;
dReal _tmp32_;
dxVector3* _tmp33_;
dReal _tmp34_;
dxVector3* _tmp35_;
dReal _tmp36_;
soyatomsAxis* _tmp37_;
soyatomsAxis* _tmp38_;
soyatomsAxis* _tmp39_;
soyatomsAxis* _tmp40_;
_tmp27_ = value;
if (_tmp27_ != NULL) {
soyatomsAxis* _tmp28_;
guint _tmp29_ = 0U;
soyatomsAxis* _tmp30_;
_tmp28_ = self->priv->_axis_obj;
g_signal_parse_name ("on-set", SOY_ATOMS_TYPE_AXIS, &_tmp29_, NULL, FALSE);
g_signal_handlers_disconnect_matched (_tmp28_, G_SIGNAL_MATCH_ID | G_SIGNAL_MATCH_FUNC | G_SIGNAL_MATCH_DATA, _tmp29_, 0, NULL, (GCallback) __soy_joints_hinge_axis_set_soy_atoms_axis_on_set, self);
_tmp30_ = self->priv->_axis_obj;
g_object_weak_unref ((GObject*) _tmp30_, __soy_joints_hinge_axis_weak_gweak_notify, self);
}
_tmp31_ = v;
_tmp32_ = _tmp31_->x;
_tmp33_ = v;
_tmp34_ = _tmp33_->y;
_tmp35_ = v;
_tmp36_ = _tmp35_->z;
_tmp37_ = soy_atoms_axis_new ((gfloat) _tmp32_, (gfloat) _tmp34_, (gfloat) _tmp36_);
_g_object_unref0 (value);
value = _tmp37_;
_tmp38_ = value;
g_signal_connect_object (_tmp38_, "on-set", (GCallback) __soy_joints_hinge_axis_set_soy_atoms_axis_on_set, self, 0);
_tmp39_ = value;
g_object_weak_ref ((GObject*) _tmp39_, __soy_joints_hinge_axis_weak_gweak_notify, self);
_tmp40_ = value;
self->priv->_axis_obj = _tmp40_;
}
result = value;
_dvector3_free0 (v);
return result;
}
soyatomsAxis* soy_joints_hinge_get_axis (soyjointsHinge* self) {
soyatomsAxis* result;
dxVector3* v = NULL;
dxVector3* _tmp0_ = NULL;
struct dxJoint* _tmp1_ = NULL;
dxVector3* _tmp2_ = NULL;
soyatomsAxis* value = NULL;
soyatomsAxis* _tmp3_ = NULL;
soyatomsAxis* _tmp4_ = NULL;
gboolean _tmp5_ = FALSE;
gboolean _tmp6_ = FALSE;
gboolean _tmp7_ = FALSE;
soyatomsAxis* _tmp8_ = NULL;
gboolean _tmp14_ = FALSE;
gboolean _tmp20_ = FALSE;
gboolean _tmp26_ = FALSE;
#line 107 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_return_val_if_fail (self != NULL, NULL);
#line 108 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp0_ = dvector3_new ();
#line 108 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
v = _tmp0_;
#line 109 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp1_ = ((soyjointsJoint*) self)->joint;
#line 109 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp2_ = v;
#line 109 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
dJointGetHingeAxis ((struct dxJoint*) _tmp1_, _tmp2_);
#line 110 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp3_ = self->priv->_axis_obj;
#line 110 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp4_ = _g_object_ref0 (_tmp3_);
#line 110 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
value = _tmp4_;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp8_ = value;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
if (_tmp8_ == NULL) {
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp7_ = TRUE;
#line 770 "Hinge.c"
} else {
dxVector3* _tmp9_ = NULL;
dReal _tmp10_ = 0.0;
soyatomsAxis* _tmp11_ = NULL;
gfloat _tmp12_ = 0.0F;
gfloat _tmp13_ = 0.0F;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp9_ = v;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp10_ = _tmp9_->x;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp11_ = value;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp12_ = soy_atoms_axis_get_x (_tmp11_);
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp13_ = _tmp12_;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp7_ = ((gfloat) _tmp10_) != _tmp13_;
#line 789 "Hinge.c"
}
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp14_ = _tmp7_;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
if (_tmp14_) {
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp6_ = TRUE;
#line 797 "Hinge.c"
} else {
dxVector3* _tmp15_ = NULL;
dReal _tmp16_ = 0.0;
soyatomsAxis* _tmp17_ = NULL;
gfloat _tmp18_ = 0.0F;
gfloat _tmp19_ = 0.0F;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp15_ = v;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp16_ = _tmp15_->y;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp17_ = value;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp18_ = soy_atoms_axis_get_y (_tmp17_);
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp19_ = _tmp18_;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp6_ = ((gfloat) _tmp16_) != _tmp19_;
#line 816 "Hinge.c"
}
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp20_ = _tmp6_;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
if (_tmp20_) {
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp5_ = TRUE;
#line 824 "Hinge.c"
} else {
dxVector3* _tmp21_ = NULL;
dReal _tmp22_ = 0.0;
soyatomsAxis* _tmp23_ = NULL;
gfloat _tmp24_ = 0.0F;
gfloat _tmp25_ = 0.0F;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp21_ = v;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp22_ = _tmp21_->z;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp23_ = value;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp24_ = soy_atoms_axis_get_z (_tmp23_);
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp25_ = _tmp24_;
#line 112 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp5_ = ((gfloat) _tmp22_) != _tmp25_;
#line 843 "Hinge.c"
}
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp26_ = _tmp5_;
#line 111 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
if (_tmp26_) {
#line 849 "Hinge.c"
soyatomsAxis* _tmp27_ = NULL;
dxVector3* _tmp31_ = NULL;
dReal _tmp32_ = 0.0;
dxVector3* _tmp33_ = NULL;
dReal _tmp34_ = 0.0;
dxVector3* _tmp35_ = NULL;
dReal _tmp36_ = 0.0;
soyatomsAxis* _tmp37_ = NULL;
soyatomsAxis* _tmp38_ = NULL;
soyatomsAxis* _tmp39_ = NULL;
soyatomsAxis* _tmp40_ = NULL;
#line 113 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp27_ = value;
#line 113 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
if (_tmp27_ != NULL) {
#line 865 "Hinge.c"
soyatomsAxis* _tmp28_ = NULL;
guint _tmp29_ = 0U;
soyatomsAxis* _tmp30_ = NULL;
#line 114 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp28_ = self->priv->_axis_obj;
#line 114 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_signal_parse_name ("on-set", SOY_ATOMS_TYPE_AXIS, &_tmp29_, NULL, FALSE);
#line 114 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_signal_handlers_disconnect_matched (_tmp28_, G_SIGNAL_MATCH_ID | G_SIGNAL_MATCH_FUNC | G_SIGNAL_MATCH_DATA, _tmp29_, 0, NULL, (GCallback) __soy_joints_hinge_axis_set_soy_atoms_axis_on_set, self);
#line 115 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp30_ = self->priv->_axis_obj;
#line 115 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_object_weak_unref ((GObject*) _tmp30_, __soy_joints_hinge_axis_weak_gweak_notify, self);
#line 879 "Hinge.c"
}
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp31_ = v;
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp32_ = _tmp31_->x;
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp33_ = v;
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp34_ = _tmp33_->y;
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp35_ = v;
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp36_ = _tmp35_->z;
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp37_ = soy_atoms_axis_new ((gfloat) _tmp32_, (gfloat) _tmp34_, (gfloat) _tmp36_);
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_g_object_unref0 (value);
#line 116 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
value = _tmp37_;
#line 119 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp38_ = value;
#line 119 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_signal_connect_object (_tmp38_, "on-set", (GCallback) __soy_joints_hinge_axis_set_soy_atoms_axis_on_set, self, 0);
#line 120 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp39_ = value;
#line 120 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_object_weak_ref ((GObject*) _tmp39_, __soy_joints_hinge_axis_weak_gweak_notify, self);
#line 121 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp40_ = value;
#line 121 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
self->priv->_axis_obj = _tmp40_;
#line 911 "Hinge.c"
}
#line 122 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
result = value;
#line 122 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_dvector3_free0 (v);
#line 122 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
return result;
#line 919 "Hinge.c"
}