void soy_joints_universal_addTorques (soyjointsUniversal* self, dReal torque1, dReal torque2) {
struct dxJoint* _tmp0_;
dReal _tmp1_;
dReal _tmp2_;
g_return_if_fail (self != NULL);
_tmp0_ = ((soyjointsJoint*) self)->joint;
_tmp1_ = torque1;
_tmp2_ = torque2;
dJointAddUniversalTorques ((struct dxJoint*) _tmp0_, _tmp1_, _tmp2_);
}
//! This function is called once per simulation step, allowing the
//! constraint to be "motorized" according to some response.
void OscUniversalODE::simulationCallback()
{
ODEConstraint& me = *static_cast<ODEConstraint*>(special());
dReal angle1 = dJointGetUniversalAngle1(me.joint());
dReal rate1 = dJointGetUniversalAngle1Rate(me.joint());
dReal addtorque1 =
- m_response->m_stiffness.m_value*angle1
- m_response->m_damping.m_value*rate1;
dReal angle2 = dJointGetUniversalAngle2(me.joint());
dReal rate2 = dJointGetUniversalAngle2Rate(me.joint());
dReal addtorque2 =
- m_response->m_stiffness.m_value*angle2
- m_response->m_damping.m_value*rate2;
m_torque1.m_value = addtorque1;
m_torque2.m_value = addtorque2;
dJointAddUniversalTorques(me.joint(), addtorque1, addtorque2);
}
