void soy_joints_slider_addForce (soyjointsSlider* self, dReal force) {
struct dxJoint* _tmp0_;
dReal _tmp1_;
g_return_if_fail (self != NULL);
_tmp0_ = ((soyjointsJoint*) self)->joint;
_tmp1_ = force;
dJointAddSliderForce ((struct dxJoint*) _tmp0_, _tmp1_);
}
/**
*@brief ジョイントの力制御の関数
* @param v トルク、力
*/
void ODEJointObj::ControlJointToq(double v)
{
ms->mu->lock();
if(JointType == 0)
{
dJointAddSliderForce(joint, v);
}
else if(JointType == 2)
{
dJointAddHingeTorque(joint, v);
}
ms->mu->unlock();
}
void ODE_LinearSpringModel::updateInputValues() {
if(mOwnerSpringAdapter == 0 || mOwnerSpringAdapter->getCurrentTargetJoint() == 0) {
return;
}
//The first time an update is run the mJoint is located.
//This has to be done here instead of setup() to ensure that the joint is
//really available, which can not be guaranteed in setup().
if(mJoint == 0) {
SimJoint *simJoint = mOwnerSpringAdapter->getCurrentTargetJoint();
ODE_Joint *odeJoint = dynamic_cast<ODE_Joint*>(simJoint);
if(odeJoint == 0) {
MotorAdapter *adapter = dynamic_cast<MotorAdapter*>(simJoint);
if(adapter != 0) {
odeJoint = dynamic_cast<ODE_Joint*>(adapter->getActiveMotorModel());
}
}
if(simJoint != 0 && odeJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
if(dynamic_cast<HingeJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
else if(getType() == MotorModel::SLIDER_JOINT) {
if(dynamic_cast<SliderJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
}
}
if(mJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
double torque = calculateTorque(dJointGetHingeAngle(mJoint));
dJointAddHingeTorque(mJoint, torque);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(torque);
}
else if(getType() == MotorModel::SLIDER_JOINT) {
double force = calculateForce(dJointGetSliderPosition(mJoint));
dJointAddSliderForce(mJoint, force);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(force);
}
}
}
void OscSlideODE::simulationCallback()
{
ODEConstraint& me = *static_cast<ODEConstraint*>(special());
dReal pos = dJointGetSliderPosition(me.joint());
dReal rate = dJointGetSliderPositionRate(me.joint());
dReal addforce =
- m_response->m_stiffness.m_value*pos
- m_response->m_damping.m_value*rate;
m_force.m_value = addforce;
dJointAddSliderForce(me.joint(), addforce);
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd)
{
case 'h' :
case 'H' :
case '?' :
printKeyBoardShortCut();
break;
// Force
case 'q' :
case 'Q' :
dBodyAddForce (body[BODY1],4,0,0);
break;
case 'w' :
case 'W' :
dBodyAddForce (body[BODY1],-4,0,0);
break;
case 'a' :
case 'A' :
dBodyAddForce (body[BODY1],0,40,0);
break;
case 's' :
case 'S' :
dBodyAddForce (body[BODY1],0,-40,0);
break;
case 'z' :
case 'Z' :
dBodyAddForce (body[BODY1],0,0,4);
break;
case 'x' :
case 'X' :
dBodyAddForce (body[BODY1],0,0,-4);
break;
// Torque
case 'e':
case 'E':
dBodyAddTorque (body[BODY1],0.1,0,0);
break;
case 'r':
case 'R':
dBodyAddTorque (body[BODY1],-0.1,0,0);
break;
case 'd':
case 'D':
dBodyAddTorque (body[BODY1],0, 0.1,0);
break;
case 'f':
case 'F':
dBodyAddTorque (body[BODY1],0,-0.1,0);
break;
case 'c':
case 'C':
dBodyAddTorque (body[BODY1],0.1,0,0);
break;
case 'v':
case 'V':
dBodyAddTorque (body[BODY1],-0.1,0,0);
break;
case 't':
case 'T':
if (joint->getType() == dJointTypePiston)
dJointAddPistonForce (joint->id(),1);
else
dJointAddSliderForce (joint->id(),1);
break;
case 'y':
case 'Y':
if (joint->getType() == dJointTypePiston)
dJointAddPistonForce (joint->id(),-1);
else
dJointAddSliderForce (joint->id(),-1);
break;
case '8' :
dJointAttach(joint->id(), body[0], 0);
break;
case '9' :
dJointAttach(joint->id(), 0, body[0]);
break;
case 'i':
case 'I' :
joint->setParam (dParamLoStop, 0);
joint->setParam (dParamHiStop, 0);
break;
case 'o':
case 'O' :
joint->setParam (dParamLoStop2, 0);
joint->setParam (dParamHiStop2, 0);
break;
case 'k':
case 'K':
joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0);
joint->setParam (dParamHiStop2, 45.0*3.14159267/180.0);
break;
case 'l':
case 'L':
joint->setParam (dParamLoStop2, -dInfinity);
joint->setParam (dParamHiStop2, dInfinity);
break;
// Velocity of joint
case ',':
case '<' :
{
dReal vel = joint->getParam (dParamVel) - VEL_INC;
joint->setParam (dParamVel, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case '.':
case '>' :
{
dReal vel = joint->getParam (dParamVel) + VEL_INC;
joint->setParam (dParamVel, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case 'p' :
case 'P' :
{
switch (joint->getType() )
{
case dJointTypeSlider :
{
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
std::cout<<"Position ="<<sj->getPosition() <<"\n";
}
break;
case dJointTypePiston :
{
dPistonJoint *rj = reinterpret_cast<dPistonJoint *> (joint);
std::cout<<"Position ="<<rj->getPosition() <<"\n";
}
break;
default:
{} // keep the compiler happy
}
}
break;
case '+' :
(++tc) %= 4;
setPositionBodies (tc);
break;
case '-' :
(--tc) %= 4;
setPositionBodies (tc);
break;
}
}
void CODESliderJoint::addForce(double force)
{
dJointAddSliderForce(mID, force);
}
void ODE_Link::setTorque(dReal t){
if(jointType == ODE_Link::ROTATIONAL_JOINT)
return dJointAddHingeTorque(odeJointId, t);
else if(jointType == ODE_Link::SLIDE_JOINT)
return dJointAddSliderForce(odeJointId, t);
}
