コード例 #1
0
ファイル: objects.cpp プロジェクト: codders/soylent
Rope::Rope(Object* obja, Body* bodya, Object* objb, Body* bodyb)
    : obja_(obja), objb_(objb), selected_(false)
{
    vec apos = bodya->position();
    proxya_.set_position(apos);
    proxya_.set_velocity(bodya->velocity());
    proxya_.set_mass(0.01, 1);
    hingea_ = dJointCreateHinge(LEVEL->world, 0);
    dJointAttach(hingea_, proxya_.body_id(), bodya->body_id());
    dJointSetHingeAxis(hingea_, 0, 0, 1);

    vec bpos = bodyb->position();
    proxyb_.set_position(bpos);
    proxyb_.set_velocity(bodyb->velocity());
    proxyb_.set_mass(0.01, 1);
    hingeb_ = dJointCreateHinge(LEVEL->world, 0);
    dJointAttach(hingeb_, proxyb_.body_id(), bodyb->body_id());
    dJointSetHingeAxis(hingeb_, 0, 0, 1);
    
    rope_ = dJointCreateSlider(LEVEL->world, 0);
    vec axis = bpos - apos;
    dJointAttach(rope_, proxya_.body_id(), proxyb_.body_id());
    dJointSetSliderAxis(rope_, axis.x, axis.y, 0);
    
    dJointSetSliderParam(rope_, dParamLoStop, 0);
    dJointSetSliderParam(rope_, dParamStopCFM, 0.25);
    dJointSetSliderParam(rope_, dParamStopERP, 0.01);

    ext_ = base_ext_ = axis.norm();
}
コード例 #2
0
void CubeBasePiece::attachToBase(dBodyID otherBody, dWorldID world, dJointGroupID jointGroup, dReal x, dReal y, dReal z, const dMatrix3 rotationMatrix)
{
    dBodySetPosition(body,x,y,z);

    dJointID connectingJoint = dJointCreateSlider(world,jointGroup);
    dJointAttach(connectingJoint,otherBody,body);
    dJointSetSliderParam(connectingJoint,dParamLoStop,0);
    dJointSetSliderParam(connectingJoint,dParamHiStop,0);
}
コード例 #3
0
ファイル: ODEJoints.cpp プロジェクト: ikoryakovskiy/grl
void CODESliderJoint::setParams()
{
	dJointSetSliderAxis(mID, mAxisX, mAxisY, mAxisZ);
	dJointSetSliderParam(mID, dParamLoStop, mLowerLimit);
	dJointSetSliderParam(mID, dParamHiStop, mUpperLimit);
	if (mStopERP >= 0)
		dJointSetSliderParam(mID, dParamStopERP, mStopERP);
	if (mStopCFM >= 0)
		dJointSetSliderParam(mID, dParamStopCFM, mStopCFM);
}
コード例 #4
0
ファイル: Joints.cpp プロジェクト: flair2005/inVRs
/**
 * This method is called if the joint should be attached.
 * It creates the ODE-joint, calculates the current
 * axis-orientation and attaches the Joint.
 * @param obj1 first ODE-object to attach with
 * @param obj2 second ODE-object to attach with
 **/
void SliderJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
	gmtl::Vec3f newAxis;
	gmtl::Quatf entityRot;
	gmtl::AxisAnglef axAng;
	gmtl::Vec3f scale = gmtl::Vec3f(1,1,1);
	TransformationData entityTrans;

	joint = dJointCreateSlider(world, 0);
	dJointAttach(joint, obj1, obj2);

	newAxis = axis;
	if (mainEntity != NULL)
	{
		entityTrans = mainEntity->getEnvironmentTransformation();
// scale Axis by mainEntity-scale value because of possible distortion
/*		scale[0] = mainEntity->getXScale();
		scale[1] = mainEntity->getYScale();
		scale[2] = mainEntity->getZScale();*/
		scale = entityTrans.scale;

		newAxis[0] *= scale[0];
		newAxis[1] *= scale[1];
		newAxis[2] *= scale[2];
		gmtl::normalize(newAxis);

// get the Rotation of the mainEntity
// 		axAng[0] = mainEntity->getRotAngle();
// 		axAng[1] = mainEntity->getXRot();
// 		axAng[2] = mainEntity->getYRot();
// 		axAng[3] = mainEntity->getZRot();
// 		gmtl::set(entityRot, axAng);
		entityRot = entityTrans.orientation;

// rotate Axis by mainEntity-rotation
		newAxis *= entityRot;
	} // if
	dJointSetSliderAxis(joint, newAxis[0], newAxis[1], newAxis[2]);

// set the minimum and maximum joint-positions (if set).
	if (posSet)
	{
		gmtl::Vec3f minDistVec = newAxis * minPos;
		gmtl::Vec3f maxDistVec = newAxis * maxPos;
		minDistVec[0] *= scale[0];
		minDistVec[1] *= scale[1];
		minDistVec[2] *= scale[2];
		maxDistVec[0] *= scale[0];
		maxDistVec[1] *= scale[1];
		maxDistVec[2] *= scale[2];
		dJointSetSliderParam(joint, dParamLoStop, gmtl::dot(minDistVec,newAxis));
		dJointSetSliderParam(joint, dParamHiStop, gmtl::dot(maxDistVec,newAxis));
	} // if
} // attachJoint
コード例 #5
0
ファイル: ODEJoints.cpp プロジェクト: ikoryakovskiy/grl
bool CODESliderJoint::init(dWorld& world, dJointGroupID groupID)
{
	bool result = CODEJoint::init(world, groupID);

	if (mDryFriction > 0)
	{
		dJointSetSliderParam(mID, dParamVel, 0);
		dJointSetSliderParam(mID, dParamFMax, mDryFriction);
	}

	return result;
}
コード例 #6
0
ファイル: ODEJointObj.cpp プロジェクト: Nobu19800/OgreRTC
/**
*@brief ジョイントの速度制御の関数
* @param v 目標速度
*/
void ODEJointObj::ControlJointVel(double v)
{
	ms->mu->lock();
	if(JointType == 0)
	{
		dJointSetSliderParam(joint, dParamVel, v);
		dJointSetSliderParam(joint, dParamFMax, 200);
	}
	else if(JointType == 2)
	{
		dJointSetHingeParam(joint,dParamVel, v);
		dJointSetHingeParam(joint,dParamFMax,50.);
	}
	ms->mu->unlock();
}
コード例 #7
0
ファイル: objects.cpp プロジェクト: codders/soylent
void Rope::lengthen(num amt) {
    ext_ += amt;
    if (ext_ < 0) {
        ext_ = 0;
    }
    dJointSetSliderParam(rope_, dParamLoStop, base_ext_ - ext_);
}
コード例 #8
0
ファイル: ServoMotor.cpp プロジェクト: RomanMichna/diplomovka
void ServoMotor::create(Joint* joint)
{
  ASSERT(dJointGetType(joint->joint) == dJointTypeHinge || dJointGetType(joint->joint) == dJointTypeSlider);
  this->joint = positionSensor.joint = joint;
  if(dJointGetType(joint->joint) == dJointTypeHinge)
    dJointSetHingeParam(joint->joint, dParamFMax, maxForce);
  else
    dJointSetSliderParam(joint->joint, dParamFMax, maxForce);
}
コード例 #9
0
ファイル: machine.cpp プロジェクト: Eliasvan/machineball
void Machine::adjustPush(double m)
{
	if(pushtime>0)
	{
		dJointSetSliderParam(joint, dParamVel, -100);
		dJointSetSliderParam(joint, dParamFMax, 5000);
	}
	else
	{
		dJointSetSliderParam(joint, dParamVel, 100);
		dJointSetSliderParam(joint, dParamFMax, 1000);
	}
	pushtime-=m;
	energy+=m;
	if(energy>4)
		energy=4;
	if(pushtime<0)
		pushtime=0;
}
コード例 #10
0
ファイル: physics.c プロジェクト: ntoand/electro
static void set_phys_joint_attr(dJointID j, int p, float v)
{
    switch (dJointGetType(j))
    {
    case dJointTypeHinge:     dJointSetHingeParam    (j, p, v); break;
    case dJointTypeSlider:    dJointSetSliderParam   (j, p, v); break;
    case dJointTypeHinge2:    dJointSetHinge2Param   (j, p, v); break;
    case dJointTypeUniversal: dJointSetUniversalParam(j, p, v); break;
    default: break;
    }
}
コード例 #11
0
ファイル: ODE_SliderJoint.cpp プロジェクト: nerd-toolkit/nerd
/**
 * Creates a new IBDS::SliderJoint.
 *
 * @param body1 the first body to connect the joint to.
 * @param body2 the second body to connect the joint to.
 * @return a new IBDS::SliderJoint.
 */
dJointID ODE_SliderJoint::createJoint(dBodyID body1, dBodyID body2) {
	if(mFirstAxisPoint->get().equals(mSecondAxisPoint->get())) {
		Core::log("Invalid axes for ODE_SliderJoint.");
		return 0;
	}
	//if one of the bodyIDs is null, the joint is connected to a static object.
	dJointID newJoint = dJointCreateSlider(mWorldID, mGeneralJointGroup);
	dJointAttach(newJoint, body1, body2);
	
	Vector3D axis(mSecondAxisPoint->getX() - mFirstAxisPoint->getX(), 
		mSecondAxisPoint->getY() - mFirstAxisPoint->getY(), 
		mSecondAxisPoint->getZ() - mFirstAxisPoint->getZ());
	axis.normalize();
	dJointSetSliderAxis(newJoint, axis.getX(), axis.getY(), axis.getZ());
	dJointSetSliderParam(newJoint, dParamLoStop, mMinPositionValue->get());
	dJointSetSliderParam(newJoint, dParamHiStop, mMaxPositionValue->get());
	dJointSetSliderParam(newJoint, dParamVel, 0.0);
	dJointSetSliderParam(newJoint, dParamFMax, mFrictionValue->get());

	return newJoint; 
}
コード例 #12
0
ファイル: Slider.cpp プロジェクト: RomanMichna/diplomovka
void Slider::createPhysics()
{
  ASSERT(axis);
  ASSERT(axis->motor);

  //
  axis->create();

  //
  ::PhysicalObject::createPhysics();

  // find bodies to connect
  Body* parentBody = dynamic_cast<Body*>(parent);
  ASSERT(!parentBody || parentBody->body);
  ASSERT(!children.empty());
  Body* childBody = dynamic_cast<Body*>(*children.begin());
  ASSERT(childBody);
  ASSERT(childBody->body);

  // create joint
  joint = dJointCreateSlider(Simulation::simulation->physicalWorld, 0);
  dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0);
  //set Slider joint parameter
  Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z);
  dJointSetSliderAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z);
  if(axis->cfm != -1.f)
    dJointSetSliderParam(joint, dParamCFM, dReal(axis->cfm));

  if(axis->deflection)
  {
    const Axis::Deflection& deflection = *axis->deflection;
    float minSliderLimit = deflection.min;
    float maxSliderLimit = deflection.max;
    if(minSliderLimit > maxSliderLimit)
      minSliderLimit = maxSliderLimit;
    //Set physical limits to higher values (+10%) to avoid strange Slider effects.
    //Otherwise, sometimes the motor exceeds the limits.
    float internalTolerance = (maxSliderLimit - minSliderLimit) * 0.1f;
    if(dynamic_cast<ServoMotor*>(axis->motor))
    {
      minSliderLimit -= internalTolerance;
      maxSliderLimit += internalTolerance;
    }
    dJointSetSliderParam(joint, dParamLoStop, dReal(minSliderLimit));
    dJointSetSliderParam(joint, dParamHiStop, dReal(maxSliderLimit));
    // this has to be done due to the way ODE sets joint stops
    dJointSetSliderParam(joint, dParamLoStop, dReal(minSliderLimit));
    if(deflection.stopCFM != -1.f)
      dJointSetSliderParam(joint, dParamStopCFM, dReal(deflection.stopCFM));
    if(deflection.stopERP != -1.f)
      dJointSetSliderParam(joint, dParamStopERP, dReal(deflection.stopERP));
  }

  // create motor
  axis->motor->create(this);

  OpenGLTools::convertTransformation(rotation, translation, transformation);
}
コード例 #13
0
void vmWishboneCar::setWheelSuspension(vm::WheelLoc loc, dReal step, dReal kps, dReal kds)
{
    vmWheel *wnow;
    vmWishbone *snow;
    switch (loc) {
    case vm::WheelLoc::FR:
        wnow= &frWheel;
        snow= &frSuspension;
        break;
    case vm::WheelLoc::FL:
        wnow= &flWheel;
        snow= &flSuspension;
        break;
    case vm::WheelLoc::RR:
        wnow= &rrWheel;
        snow= &rrSuspension;
        break;
    case vm::WheelLoc::RL:
        wnow= &rlWheel;
        snow= &rlSuspension;
        break;
    default:
        break;
    }

    dJointSetSliderParam(snow->jStrutMid, dParamHiStop, 0.0);
    dJointSetSliderParam(snow->jStrutMid, dParamLoStop, 0.0);
    dJointSetSliderParam(snow->jStrutMid, dParamStopCFM, computeCFM(step,kps,kds));
    dJointSetSliderParam(snow->jStrutMid, dParamStopERP, computeERP(step,kps,kds));
    dJointSetSliderParam(snow->jStrutMid, dParamFMax, dInfinity);

    /*dJointSetLMotorParam(snow->jLowSpring, dParamHiStop, 0.0);
    dJointSetLMotorParam(snow->jLowSpring, dParamLoStop, 0.0);
    dJointSetLMotorParam(snow->jLowSpring, dParamStopCFM, computeCFM(step,kps,kds));
    dJointSetLMotorParam(snow->jLowSpring, dParamStopERP, computeERP(step,kps,kds));
    dJointSetLMotorParam(snow->jLowSpring, dParamFMax, dInfinity);*/
}
コード例 #14
0
ファイル: ServoMotor.cpp プロジェクト: RomanMichna/diplomovka
void ServoMotor::act()
{
  const float currentPos = dJointGetType(joint->joint) == dJointTypeHinge
                           ? dJointGetHingeAngle(joint->joint)
                           : dJointGetSliderPosition(joint->joint);

  float setpoint = this->setpoint;
  const float maxValueChange = maxVelocity * Simulation::simulation->scene->stepLength;
  if(std::abs(setpoint - currentPos) > maxValueChange)
  {
    if(setpoint < currentPos)
      setpoint = currentPos - maxValueChange;
    else
      setpoint = currentPos + maxValueChange;
  }

  const float newVel = controller.getOutput(currentPos, setpoint);
  if(dJointGetType(joint->joint) == dJointTypeHinge)
    dJointSetHingeParam(joint->joint, dParamVel, dReal(newVel));
  else
    dJointSetSliderParam(joint->joint, dParamVel, dReal(newVel));
}
コード例 #15
0
		//SetAllMovParams
		void JointSlider::SetAllMovParams(double LoStop, double HiStop,
										  double Velocity, double MaxForce,
										  double FudgeFactor, double Bounce,
										  double StopERP, double StopCFM)
		{
			if (LoStop <= 0) 
				dJointSetHingeParam(this->_id, dParamLoStop, LoStop);

			if (HiStop >= 0)
				dJointSetHingeParam(this->_id, dParamHiStop, HiStop);

			dJointSetSliderParam(this->_id, dParamVel, Velocity);
			dJointSetSliderParam(this->_id, dParamFMax, MaxForce);
			dJointSetSliderParam(this->_id, dParamFudgeFactor, FudgeFactor);
			dJointSetSliderParam(this->_id, dParamBounce, Bounce);
			dJointSetSliderParam(this->_id, dParamStopERP, StopERP);
			dJointSetSliderParam(this->_id, dParamStopCFM, StopCFM);
		}
コード例 #16
0
ファイル: machine.cpp プロジェクト: Eliasvan/machineball
void Machine::init()
{
	int i;
	pushtime=0;
	energy=4;
	dMass m;
	for(i=0; i<3; i++)
	{
		wheel[i] = dBodyCreate(world);
		dMassSetSphere(&m, 1, 5);
		dMassAdjust(&m, 2);
		dBodySetMass(wheel[i], &m);
		sphere[i] = dCreateSphere(0, 5);
		dGeomSetBody(sphere[i], wheel[i]);
	}
	dBodySetPosition(wheel[0], 0, 12, 6);
	dBodySetPosition(wheel[1], -6, -7, 6);
	dBodySetPosition(wheel[2], 6, -7, 6);

	body[0] = dBodyCreate(world);
	dMassSetBox(&m, 1, 20, 80, 5);
	dMassAdjust(&m, 5);
	dBodySetMass(body[0], &m);
	dBodySetPosition(body[0], 0, 0, 6.5);
	geom[0] = dCreateBox(0, 19, 27, 10);
	dGeomSetBody(geom[0], body[0]);

	body[1] = dBodyCreate(world);
	dMassSetBox(&m, 1, 11, 5, 10);
	dMassAdjust(&m, 0.3);
	dBodySetMass(body[1], &m);
	dBodySetPosition(body[1], 0, 17, 6.5);
	geom[1] = dCreateBox(0, 11, 5, 10);
	dGeomSetBody(geom[1], body[1]);

	joint = dJointCreateSlider(world, 0);
	dJointAttach(joint, body[0], body[1]);
	dJointSetSliderAxis(joint, 0, 1, 0);
	dJointSetSliderParam(joint, dParamLoStop, -9);
	dJointSetSliderParam(joint, dParamHiStop, 0);

	for(i=0; i<2; i++)
	{
		geom[i+2] = dCreateGeomTransform(0);
		dGeomTransformSetCleanup(geom[i+2], 1);
		
		finE[i] = dCreateBox(0, 7, 5, 10);
		dGeomSetPosition(finE[i], i==0?-6.3:6.3, -2, 0);
		dMatrix3 R;
		dRFromAxisAndAngle(R, 0, 0, 1, i==0?M_PI/4:-M_PI/4);
		dGeomSetRotation(finE[i], R);
		dGeomTransformSetGeom(geom[i+2], finE[i]);
		dGeomSetBody(geom[i+2], body[1]);
	}
	
	
	for(i=0; i<3; i++)
	{
		wheeljoint[i] = dJointCreateHinge2(world, 0);
		dJointAttach(wheeljoint[i], body[0], wheel[i]);
		const dReal *wPos = dBodyGetPosition(wheel[i]);
		dJointSetHinge2Anchor(wheeljoint[i], wPos[0], wPos[1], wPos[2]);
		dJointSetHinge2Axis1(wheeljoint[i], 0, 0, 1);
		dJointSetHinge2Axis2(wheeljoint[i], 1, 0, 0);

		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionERP, 0.8);
		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionCFM, 0.01);
		dJointSetHinge2Param(wheeljoint[i], dParamLoStop, 0);
		dJointSetHinge2Param(wheeljoint[i], dParamHiStop, 0);
		dJointSetHinge2Param(wheeljoint[i], dParamCFM, 0.0001);
		dJointSetHinge2Param(wheeljoint[i], dParamStopERP, 0.8);
		dJointSetHinge2Param(wheeljoint[i], dParamStopCFM, 0.0001);
	}

	reset();
}
コード例 #17
0
int setupTest (int n)
{
  switch (n) {

  // ********** fixed joint

  case 0: {			// 2 body
    constructWorldForTest (0,2,
			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
			   1,1,0, 1,1,0,
			   0.25*M_PI,0.25*M_PI);
    joint = dJointCreateFixed (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetFixed (joint);
    return 1;
  }

  case 1: {			// 1 body to static env
    constructWorldForTest (0,1,
			   0.5*SIDE,0.5*SIDE,1, 0,0,0,
			   1,0,0, 1,0,0,
			   0,0);
    joint = dJointCreateFixed (world,0);
    dJointAttach (joint,body[0],0);
    dJointSetFixed (joint);
    return 1;
  }

  case 2: {			// 2 body with relative rotation
    constructWorldForTest (0,2,
			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
			   1,1,0, 1,1,0,
			   0.25*M_PI,-0.25*M_PI);
    joint = dJointCreateFixed (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetFixed (joint);
    return 1;
  }

  case 3: {			// 1 body to static env with relative rotation
    constructWorldForTest (0,1,
			   0.5*SIDE,0.5*SIDE,1, 0,0,0,
			   1,0,0, 1,0,0,
			   0.25*M_PI,0);
    joint = dJointCreateFixed (world,0);
    dJointAttach (joint,body[0],0);
    dJointSetFixed (joint);
    return 1;
  }

  // ********** hinge joint

  case 200:			// 2 body
    constructWorldForTest (0,2,
			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
			   1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
    joint = dJointCreateHinge (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetHingeAnchor (joint,0,0,1);
    dJointSetHingeAxis (joint,1,-1,1.41421356);
    return 1;

  case 220:			// hinge angle polarity test
  case 221:			// hinge angle rate test
    constructWorldForTest (0,2,
			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateHinge (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetHingeAnchor (joint,0,0,1);
    dJointSetHingeAxis (joint,0,0,1);
    max_iterations = 50;
    return 1;

  case 230:			// hinge motor rate (and polarity) test
  case 231:			// ...with stops
    constructWorldForTest (0,2,
			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateHinge (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetHingeAnchor (joint,0,0,1);
    dJointSetHingeAxis (joint,0,0,1);
    dJointSetHingeParam (joint,dParamFMax,1);
    if (n==231) {
      dJointSetHingeParam (joint,dParamLoStop,-0.5);
      dJointSetHingeParam (joint,dParamHiStop,0.5);
    }
    return 1;

  case 250:			// limit bounce test (gravity down)
  case 251: {			// ...gravity up
    constructWorldForTest ((n==251) ? 0.1 : -0.1, 2,
			   0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateHinge (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetHingeAnchor (joint,0,0,1);
    dJointSetHingeAxis (joint,0,1,0);
    dJointSetHingeParam (joint,dParamLoStop,-0.9);
    dJointSetHingeParam (joint,dParamHiStop,0.7854);
    dJointSetHingeParam (joint,dParamBounce,0.5);
    // anchor 2nd body with a fixed joint
    dJointID j = dJointCreateFixed (world,0);
    dJointAttach (j,body[1],0);
    dJointSetFixed (j);
    return 1;
  }

  // ********** slider

  case 300:			// 2 body
    constructWorldForTest (0,2,
			   0,0,1, 0.2,0.2,1.2,
			   0,0,1, -1,1,0, 0,0.25*M_PI);
    joint = dJointCreateSlider (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetSliderAxis (joint,1,1,1);
    return 1;

  case 320:			// slider angle polarity test
  case 321:			// slider angle rate test
    constructWorldForTest (0,2,
			   0,0,1, 0,0,1.2,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateSlider (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetSliderAxis (joint,0,0,1);
    max_iterations = 50;
    return 1;

  case 330:			// slider motor rate (and polarity) test
  case 331:			// ...with stops
    constructWorldForTest (0, 2,
			   0,0,1, 0,0,1.2,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateSlider (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetSliderAxis (joint,0,0,1);
    dJointSetSliderParam (joint,dParamFMax,100);
    if (n==331) {
      dJointSetSliderParam (joint,dParamLoStop,-0.4);
      dJointSetSliderParam (joint,dParamHiStop,0.4);
    }
    return 1;

  case 350:			// limit bounce tests
  case 351: {
    constructWorldForTest ((n==351) ? 0.1 : -0.1, 2,
			   0,0,1, 0,0,1.2,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateSlider (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetSliderAxis (joint,0,0,1);
    dJointSetSliderParam (joint,dParamLoStop,-0.5);
    dJointSetSliderParam (joint,dParamHiStop,0.5);
    dJointSetSliderParam (joint,dParamBounce,0.5);
    // anchor 2nd body with a fixed joint
    dJointID j = dJointCreateFixed (world,0);
    dJointAttach (j,body[1],0);
    dJointSetFixed (j);
    return 1;
  }

  // ********** hinge-2 joint

  case 420:			// hinge-2 steering angle polarity test
  case 421:			// hinge-2 steering angle rate test
    constructWorldForTest (0,2,
			   0.5*SIDE,0,1, -0.5*SIDE,0,1,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateHinge2 (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
    dJointSetHinge2Axis1 (joint,0,0,1);
    dJointSetHinge2Axis2 (joint,1,0,0);
    max_iterations = 50;
    return 1;

  case 430:			// hinge 2 steering motor rate (+polarity) test
  case 431:			// ...with stops
  case 432:			// hinge 2 wheel motor rate (+polarity) test
    constructWorldForTest (0,2,
			   0.5*SIDE,0,1, -0.5*SIDE,0,1,
			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateHinge2 (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
    dJointSetHinge2Axis1 (joint,0,0,1);
    dJointSetHinge2Axis2 (joint,1,0,0);
    dJointSetHinge2Param (joint,dParamFMax,1);
    dJointSetHinge2Param (joint,dParamFMax2,1);
    if (n==431) {
      dJointSetHinge2Param (joint,dParamLoStop,-0.5);
      dJointSetHinge2Param (joint,dParamHiStop,0.5);
    }
    return 1;

  // ********** angular motor joint

  case 600:			// test euler angle calculations
    constructWorldForTest (0,2,
			   -SIDE*0.5,0,1, SIDE*0.5,0,1,
			   0,0,1, 0,0,1, 0,0);
    joint = dJointCreateAMotor (world,0);
    dJointAttach (joint,body[0],body[1]);

    dJointSetAMotorNumAxes (joint,3);
    dJointSetAMotorAxis (joint,0,1, 0,0,1);
    dJointSetAMotorAxis (joint,2,2, 1,0,0);
    dJointSetAMotorMode (joint,dAMotorEuler);
    max_iterations = 200;
    return 1;

    // ********** universal joint

  case 700:			// 2 body
  case 701:
  case 702:
    constructWorldForTest (0,2,
 			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
 			   1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
    joint = dJointCreateUniversal (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetUniversalAnchor (joint,0,0,1);
    dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356);
    dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356);
    return 1;

  case 720:		// universal transmit torque test
  case 721:
  case 722:
  case 730:		// universal torque about axis 1
  case 731:
  case 732:
  case 740:		// universal torque about axis 2
  case 741:
  case 742:
    constructWorldForTest (0,2,
 			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
 			   1,0,0, 1,0,0, 0,0);
    joint = dJointCreateUniversal (world,0);
    dJointAttach (joint,body[0],body[1]);
    dJointSetUniversalAnchor (joint,0,0,1);
    dJointSetUniversalAxis1 (joint,0,0,1);
    dJointSetUniversalAxis2 (joint, 1, -1,0);
    max_iterations = 100;
    return 1;
  }
  return 0;
}
コード例 #18
0
dReal doStuffAndGetError (int n)
{
  switch (n) {

  // ********** fixed joint

  case 0: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the orientations are the same
    const dReal *R1 = dBodyGetRotation (body[0]);
    const dReal *R2 = dBodyGetRotation (body[1]);
    dReal err1 = dMaxDifference (R1,R2,3,3);
    // check the body offset is correct
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return (err1 + length (pp)) * 300;
  }

  case 1: {			// 1 body to static env
    addOscillatingTorque (0.1);

    // check the orientation is the identity
    dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return (err1 + length (p)) * 1e6;
  }

  case 2: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the body offset is correct
    // Should really check body rotation too.  Oh well.
    const dReal *R1 = dBodyGetRotation (body[0]);
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return length(pp) * 300;
  }

  case 3: {			// 1 body to static env with relative rotation
    addOscillatingTorque (0.1);

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return  length (p) * 1e6;
  }


  // ********** hinge joint

  case 200:			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    return dInfinity;

  case 220:			// hinge angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHingeAngle (joint);
      if (a > 0.5 && a < 1) return 0; else return 10;
    }
    return 0;

  case 221: {			// hinge angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHingeAngle (joint);
    dReal r = dJointGetHingeAngleRate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er) * 4e4;
  }

  case 230:			// hinge motor rate (and polarity) test
  case 231: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHingeAngleRate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHingeParam (joint,dParamVel,cos(a));
    if (n==231) return dInfinity;
    return err * 1e6;
  }

  // ********** slider joint

  case 300:			// 2 body
    addOscillatingTorque (0.05);
    dampRotationalMotion (0.1);
    addSpringForce (0.5);
    return dInfinity;

  case 320:			// slider angle polarity test
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    if (iteration == 40) {
      dReal a = dJointGetSliderPosition (joint);
      if (a > 0.2 && a < 0.5) return 0; else return 10;
      return a;
    }
    return 0;

  case 321: {			// slider angle rate test
    static dReal last_pos = 0;
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    dReal p = dJointGetSliderPosition (joint);
    dReal r = dJointGetSliderPositionRate (joint);
    dReal er = (p-last_pos)/STEPSIZE;	// estimated rate (almost exact)
    last_pos = p;
    return fabs(r-er) * 1e9;
  }

  case 330:			// slider motor rate (and polarity) test
  case 331: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetSliderPositionRate (joint);
    dReal err = fabs (0.7*cos(a) - r);
    if (a < 0.04) err = 0;
    a += 0.03;
    dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
    if (n==331) return dInfinity;
    return err * 1e6;
  }

  // ********** hinge-2 joint

  case 420:			// hinge-2 steering angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHinge2Angle1 (joint);
      if (a > 0.5 && a < 0.6) return 0; else return 10;
    }
    return 0;

  case 421: {			// hinge-2 steering angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHinge2Angle1 (joint);
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er)*2e4;
  }

  case 430:			// hinge 2 steering motor rate (+polarity) test
  case 431: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel,cos(a));
    if (n==431) return dInfinity;
    return err * 1e6;
  }

  case 432: {			// hinge 2 wheel motor rate (+polarity) test
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle2Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel2,cos(a));
    return err * 1e6;
  }

  // ********** angular motor joint

  case 600: {			// test euler angle calculations
    // desired euler angles from last iteration
    static dReal a1,a2,a3;

    // find actual euler angles
    dReal aa1 = dJointGetAMotorAngle (joint,0);
    dReal aa2 = dJointGetAMotorAngle (joint,1);
    dReal aa3 = dJointGetAMotorAngle (joint,2);
    // printf ("actual  = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);

    dReal err = dInfinity;
    if (iteration > 0) {
      err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
      err *= 1e10;
    }

    // get random base rotation for both bodies
    dMatrix3 Rbase;
    dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
			3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
    dBodySetRotation (body[0],Rbase);

    // rotate body 2 by random euler angles w.r.t. body 1
    a1 = 3.14 * 2 * (dRandReal()-0.5);
    a2 = 1.57 * 2 * (dRandReal()-0.5);
    a3 = 3.14 * 2 * (dRandReal()-0.5);
    dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
    dRFromAxisAndAngle (R1,0,0,1,-a1);
    dRFromAxisAndAngle (R2,0,1,0,a2);
    dRFromAxisAndAngle (R3,1,0,0,-a3);
    dMultiply0 (Rtmp1,R2,R3,3,3,3);
    dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
    dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
    dBodySetRotation (body[1],Rtmp1);
    // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);

    return err;
  }

  // ********** universal joint

  case 700: {		// 2 body: joint constraint
    dVector3 ax1, ax2;

    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 701: {		// 2 body: angle 1 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 702: {		// 2 body: angle 2 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 720: {		// universal transmit torque test: constraint error
    dVector3 ax1, ax2;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 721: {		// universal transmit torque test: angle1 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 722: {		// universal transmit torque test: angle2 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 730:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 731:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 2e3;
  }

  case 732:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 740:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 741:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 742:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e4;
  }
  }

  return dInfinity;
}
コード例 #19
0
ファイル: demo_feedback.cpp プロジェクト: BenitoJedai/jslibs
int main (int argc, char **argv)
{
  dMass m;

  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

  // create world
  dInitODE2(0);
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-9.8);
  dWorldSetQuickStepNumIterations (world, 20);

  int i;
  for (i=0; i<SEGMCNT; i++)
  {
    segbodies[i] = dBodyCreate (world);
    dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
    dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
    dBodySetMass (segbodies[i], &m);
    seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
    dGeomSetBody (seggeoms[i], segbodies[i]);
    dSpaceAdd (space, seggeoms[i]);
  }

  for (i=0; i<SEGMCNT-1; i++)
  {
    hinges[i] = dJointCreateHinge (world,0);
    dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
    dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
    dJointSetHingeAxis (hinges[i], 0,1,0);
    dJointSetHingeParam (hinges[i],dParamFMax,  8000.0);
    // NOTE:
    // Here we tell ODE where to put the feedback on the forces for this hinge
    dJointSetFeedback (hinges[i], jfeedbacks+i);
    stress[i]=0;
  }

  for (i=0; i<STACKCNT; i++)
  {
    stackbodies[i] = dBodyCreate(world);
    dMassSetBox (&m, 2.0, 2, 2, 0.6);
    dBodySetMass(stackbodies[i],&m);

    stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
    dGeomSetBody(stackgeoms[i], stackbodies[i]);
    dBodySetPosition(stackbodies[i], 0,0,8+2*i);
    dSpaceAdd(space, stackgeoms[i]);
  }

  sliders[0] = dJointCreateSlider (world,0);
  dJointAttach(sliders[0], segbodies[0], 0);
  dJointSetSliderAxis (sliders[0], 1,0,0);
  dJointSetSliderParam (sliders[0],dParamFMax,  4000.0);
  dJointSetSliderParam (sliders[0],dParamLoStop,   0.0);
  dJointSetSliderParam (sliders[0],dParamHiStop,   0.2);

  sliders[1] = dJointCreateSlider (world,0);
  dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
  dJointSetSliderAxis (sliders[1], 1,0,0);
  dJointSetSliderParam (sliders[1],dParamFMax,  4000.0);
  dJointSetSliderParam (sliders[1],dParamLoStop,   0.0);
  dJointSetSliderParam (sliders[1],dParamHiStop,  -0.2);

  groundgeom = dCreatePlane(space, 0,0,1,0);

  for (i=0; i<SEGMCNT; i++)
    colours[i]=0.0;

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupEmpty (contactgroup);
  dJointGroupDestroy (contactgroup);

  // First destroy seggeoms, then space, then the world.
  for (i=0; i<SEGMCNT; i++)
    dGeomDestroy (seggeoms[i]);
  for (i=0; i<STACKCNT; i++)
    dGeomDestroy (stackgeoms[i]);

  dSpaceDestroy(space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
コード例 #20
0
void PhysicsSliderJoint::changed(ConstFieldMaskArg whichField, 
                            UInt32            origin,
                            BitVector         details)
{
    //Do not respond to changes that have a Sync origin
    if(origin & ChangedOrigin::Sync)
    {
        return;
    }

    if(whichField & WorldFieldMask)
    {
        if(_JointID)
        {
            dJointDestroy(_JointID);
            _JointID = dJointCreateSlider(getWorld()->getWorldID(), 0);
        }
        else
        {
            _JointID = dJointCreateSlider(getWorld()->getWorldID(), 0);
            if(!(whichField & HiStopFieldMask))
            {
                setHiStop(dJointGetSliderParam(_JointID,dParamHiStop));
            }
            if(!(whichField & LoStopFieldMask))
            {
                setLoStop(dJointGetSliderParam(_JointID,dParamLoStop));
            }
            if(!(whichField & BounceFieldMask))
            {
                setBounce(dJointGetSliderParam(_JointID,dParamBounce));
            }
            if(!(whichField & CFMFieldMask))
            {
                setCFM(dJointGetSliderParam(_JointID,dParamCFM));
            }
            if(!(whichField & StopCFMFieldMask))
            {
                setStopCFM(dJointGetSliderParam(_JointID,dParamStopCFM));
            }
            if(!(whichField & StopERPFieldMask))
            {
                setStopERP(dJointGetSliderParam(_JointID,dParamStopERP));
            }
        }
    }

    Inherited::changed(whichField, origin, details);

    if((whichField & AxisFieldMask) || (whichField & WorldFieldMask))
    {
	    dJointSetSliderAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z());
    }
    if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetSliderParam(_JointID,  dParamHiStop, getHiStop());
    }
    if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetSliderParam(_JointID,  dParamLoStop, getLoStop());
    }
    if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetSliderParam(_JointID,  dParamBounce, getBounce());
    }
    if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetSliderParam(_JointID,  dParamCFM, getCFM());
    }
    if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetSliderParam(_JointID,  dParamStopERP, getStopERP());
    }
    if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetSliderParam(_JointID,  dParamStopCFM, getStopCFM());
    }
}
コード例 #21
0
WorldPhysics::WorldPhysics() {

  enable_complex=0;
  bulldozer_state=1;
  tmp_scalar=0;
  tmp_wait=0;

  qsrand(QTime::currentTime().msec());

  dInitODE();
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-9.81);
  //ground_cheat = dCreatePlane (space,0,0,1,0);
  wall1=dCreatePlane (space,-1,0,0,-100);
  wall2=dCreatePlane (space,1,0,0,0);
  wall3=dCreatePlane (space,0,-1,0,-100);
  wall4=dCreatePlane (space,0,1,0,0);
  
  
  
  // our heightfield floor

  dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();

	// Create an finite heightfield.
  dGeomHeightfieldDataBuildCallback( heightid, NULL, near_heightfield_callback,
                                     HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP,
                                     REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 );

	// Give some very bounds which, while conservative,
	// makes AABB computation more accurate than +/-INF.
  //dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) );

  gheight = dCreateHeightfield( space, heightid, 1 );
  
  
  
  // Rotate so Z is up, not Y (which is the default orientation)
  dMatrix3 R;
  dRSetIdentity( R );
  dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );
  dGeomSetRotation( gheight, R );
  dGeomSetPosition( gheight, 50,50,0 );

//  for (int  i=0;i<MAX_ITEMS;i++) {
  //  items.push_back(generateItem());
  //}
  generateItems();

  bulldozer_space = dSimpleSpaceCreate(space);
  dSpaceSetCleanup (bulldozer_space,0);
  
  bulldozer=new BoxItem(world,bulldozer_space,LENGTH,WIDTH,HEIGHT,CMASS);
  bulldozer->setPosition(STARTX,STARTY,STARTZ);

  bulldozer_cabin=new BoxItem(world,bulldozer_space,LENGTH/2,WIDTH/2,2*HEIGHT,CMASS/3);
  bulldozer_cabin->setPosition(-LENGTH/4+STARTX,STARTY,3.0/2.0*HEIGHT+STARTZ);

  bulldozer_bucket_c=new BoxItem(world,bulldozer_space,BUCKET_LENGTH,BUCKET_WIDTH,BUCKET_HEIGHT,CMASS/10);
  bulldozer_bucket_c->setPosition(LENGTH/2+BUCKET_LENGTH/2+RADIUS+STARTX,STARTY,STARTZ);
  
  bulldozer_bucket_l=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20);
  bulldozer_bucket_l->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,-BUCKET_WIDTH/2+BUCKET_LENGTH/2+STARTY,STARTZ);
  
  
  bulldozer_bucket_r=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20);
  bulldozer_bucket_r->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,BUCKET_WIDTH/2-BUCKET_LENGTH/2+STARTY,STARTZ);
  
  
  for (int i=0; i<4; i++) {
    dQuaternion q;
    dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
    wheels[i] = new WheelItem(world,bulldozer_space,q,RADIUS,WMASS);
  }
  dBodySetPosition (wheels[0]->body,0.5*LENGTH+STARTX,WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
  dBodySetPosition (wheels[1]->body,0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
  dBodySetPosition (wheels[2]->body,-0.5*LENGTH+STARTX, WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
  dBodySetPosition (wheels[3]->body,-0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);

  cabin_joint=dJointCreateSlider(world,0);
  dJointAttach(cabin_joint,bulldozer->body,bulldozer_cabin->body);
  dJointSetSliderAxis(cabin_joint,0,0,1);
  dJointSetSliderParam(cabin_joint,dParamLoStop,0);
  dJointSetSliderParam(cabin_joint,dParamHiStop,0);

  bucket_joint_c=dJointCreateSlider(world,0);
  dJointAttach(bucket_joint_c,bulldozer->body,bulldozer_bucket_c->body);
  dJointSetSliderAxis(bucket_joint_c,0,0,1);
  dJointSetSliderParam(bucket_joint_c,dParamLoStop,0);
  dJointSetSliderParam(bucket_joint_c,dParamHiStop,0);
  
  bucket_joint_l=dJointCreateSlider(world,0);
  dJointAttach(bucket_joint_l,bulldozer->body,bulldozer_bucket_l->body);
  dJointSetSliderAxis(bucket_joint_l,0,0,1);
  dJointSetSliderParam(bucket_joint_l,dParamLoStop,0);
  dJointSetSliderParam(bucket_joint_l,dParamHiStop,0);
  
  bucket_joint_r=dJointCreateSlider(world,0);
  dJointAttach(bucket_joint_r,bulldozer->body,bulldozer_bucket_r->body);
  dJointSetSliderAxis(bucket_joint_r,0,0,1);
  dJointSetSliderParam(bucket_joint_r,dParamLoStop,0);
  dJointSetSliderParam(bucket_joint_r,dParamHiStop,0);

  // front and back wheel hinges
  for (int i=0; i<4; i++) {
    wheelJoints[i] = dJointCreateHinge2 (world,0);
    dJointAttach (wheelJoints[i],bulldozer->body,wheels[i]->body);
    const dReal *a = dBodyGetPosition (wheels[i]->body);
    dJointSetHinge2Anchor (wheelJoints[i],a[0],a[1],a[2]);
    dJointSetHinge2Axis1 (wheelJoints[i],0,0,1);
    dJointSetHinge2Axis2 (wheelJoints[i],0,1,0);
  }
  // seeting ERP & CRM
  for (int i=0; i<4; i++) {
    dJointSetHinge2Param (wheelJoints[i],dParamSuspensionERP,0.5);
    dJointSetHinge2Param (wheelJoints[i],dParamSuspensionCFM,0.8);
  }
  // block back axis !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  for (int i=0; i<2; i++) {
    dJointSetHinge2Param (wheelJoints[i],dParamLoStop,0);
    dJointSetHinge2Param (wheelJoints[i],dParamHiStop,0);
  }

}