Example #1
0
static void set_phys_joint_anchor(dJointID j, const float v[3])
{
    switch (dJointGetType(j))
    {
    case dJointTypeBall:
        dJointSetBallAnchor     (j, v[0], v[1], v[2]); break;
    case dJointTypeHinge:
        dJointSetHingeAnchor    (j, v[0], v[1], v[2]); break;
    case dJointTypeHinge2:
        dJointSetHinge2Anchor   (j, v[0], v[1], v[2]); break;
    case dJointTypeUniversal:
        dJointSetUniversalAnchor(j, v[0], v[1], v[2]); break;
    default: break;
    }
}
Example #2
0
OscBallJointODE::OscBallJointODE(dWorldID odeWorld, dSpaceID odeSpace,
                                 const char *name, OscBase *parent,
                                 OscObject *object1, OscObject *object2,
                                 double x, double y, double z)
    : OscBallJoint(name, parent, object1, object2, x, y, z)
{
    dJointID odeJoint = dJointCreateBall(odeWorld,0);

    m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
                                   object1, object2);

    dJointSetBallAnchor(odeJoint, x, y, z);

    printf("[%s] Ball joint created between %s and %s at (%f,%f,%f)\n",
           simulation()->type_str(),
           object1->c_name(), object2?object2->c_name():"world",
           x, y, z);
}
Example #3
0
sODEJoint* sODEJoint::AddBallJoint( int id, dBodyID body1, dBodyID body2, float x, float y, float z )
{
	sODEJoint *pJoint = new sODEJoint( );

	//modify prev and next pointers of any involved joints, make new joint the new head
	pJoint->pNextJoint = pODEJointList;
	if ( pODEJointList ) pODEJointList->pPrevJoint = pJoint;
	pODEJointList = pJoint;

	//create the ODE hinge joint
	pJoint->oJoint = dJointCreateBall( g_ODEWorld, 0 );
	dJointAttach( pJoint->oJoint, body1, body2 );
	dJointSetBallAnchor( pJoint->oJoint, x,y,z );

	pJoint->iID = id;

	return pJoint;
}
Example #4
0
int main( void ) {
    dWorldID	world = dWorldCreate();
    dJointID	joint = dJointCreateBall( world, 0 );
    dVector3	pos;

    printf( "Create world and joint. Setting joint anchor to [4,11.18,-1.2]...\n" );
    dJointSetBallAnchor( joint, 4.0, 11.18, -1.2 );

    printf( "Done. Fetching set values...\n" );
    dJointGetBallAnchor( joint, pos );

    printf( "Anchor is at: [%0.5f, %0.5f, %0.5f]", pos[0], pos[1], pos[2] );

    dJointDestroy( joint );
    dWorldDestroy( world );

    return 0;
}
Example #5
0
/**
 * This method is called if the joint should be attached.
 * It creates the ODE-joint, calculates the current anchor-position
 * and attaches the Joint.
 * @param obj1 first ODE-object to attach with
 * @param obj2 second ODE-object to attach with
 **/
void BallJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
	gmtl::Vec3f newAnchor;
	gmtl::Quatf entityRot;
	gmtl::AxisAnglef axAng;
	TransformationData entityTrans;

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

	newAnchor = anchor;
	if (mainEntity != NULL)
	{
// scale Anchor-offset by mainEntity-scale value
		entityTrans = mainEntity->getEnvironmentTransformation();
/*		newAnchor[0] *= mainEntity->getXScale();
		newAnchor[1] *= mainEntity->getYScale();
		newAnchor[2] *= mainEntity->getZScale();*/
		newAnchor[0] *= entityTrans.scale[0];
		newAnchor[1] *= entityTrans.scale[1];
		newAnchor[2] *= entityTrans.scale[2];

// 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 Anchor-offset by mainEntity-rotation
		newAnchor *= entityRot;

// transform new Anchor to world coordinates
// 		newAnchor[0] += mainEntity->getXTrans();
// 		newAnchor[1] += mainEntity->getYTrans();
// 		newAnchor[2] += mainEntity->getZTrans();
		newAnchor += entityTrans.position;

	}
	dJointSetBallAnchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
} // attachJoint
//creates ball joint on base
void PhysicsActor::makeJoint(){

        if (!base)
            return;

        PhysicsActor* physBase=dynamic_cast<PhysicsActor*>(base);

        if (!physBase)
            return;

        cout << "creating joint..." << endl;

        const dReal* baseLoc=dBodyGetPosition(physBase->body);
        // create the joint
        joint = dJointCreateBall(renderer->physicsWorld,0);

		//joint->setAnchor((baseLoc[0] + myLoc[0])*0.5 ,(baseLoc[1] + myLoc[1]) * 0.5 ,(baseLoc[2] + myLoc[2]) * 0.5 );
		dJointAttach(joint, body, physBase->body);

        // And lastly we tell ODE where exactly the two objects are joined in world coordinates.
        dJointSetBallAnchor(joint, baseLoc[0], baseLoc[1], baseLoc[2]);

        bJointedToBase=true;
}
Example #7
0
/**
 *	This method updates the position and orientation of the grabbed
 *	object and all connected objects. If the grabbed object has
 *	no connections to other objects (via joints) the method simply
 *	applies the passed position and orientation to the grabbed object.
 *	Otherwise the calculation of the new transformation takes place in
 *	two steps:
 *	First the orientational difference between the grabbed object and
 *	and the passed orientation is calculated and applied to the object
 *	as torque. Then a simulation-step takes place, where the timestep
 *	is set to 0.5, what should leed to the half orientation correction.
 *	After the simulation step the objects velocities are set to zero.
 *	In the second step the difference of the objects position and the
 *	passed one are calculated and applied to the object as force. Like
 *	in step one a simulation-step with dt=0.5 takes place and the
 *	velocities are set to zero again.
 *	After the computation of the new transformations, the method iterates
 *	over all objects, which are grabbed or are linked with the grabbed
 *	object and copies their transformation from the ODE-representation
 *	to the Entity-members.
 *	At last the method calls the checkConstraints-method of all currently
 *	used joints, so that the joints can be activated or deactivated.
 *	@param position: position of the manipulating object
 *	@param orientation: orientation of the manipulating object
 */
TransformationData JointInteraction::update(TransformationData trans)
{
	if (!isInitialized)
		init();

	gmtl::AxisAnglef AxAng;
	gmtl::Vec3f diff;
	dQuaternion quat;

	userTrans.position = trans.position;
	userTrans.orientation = trans.orientation;

	if (!isGrabbing)
		return identityTransformation();

	int numJoints = dBodyGetNumJoints(grabbedObject->body);
	if (!numJoints)
	{ // Simple code for objects without Joints
		convert(quat, userTrans.orientation);
		dBodySetPosition(grabbedObject->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]);
		dBodySetQuaternion(grabbedObject->body, quat);
	}
	else
	{ // Code for object with Joints
		TransformationData objectTrans;
		gmtl::Quatf rotation;
		gmtl::Vec3f force, torque;

		const dReal* pos = dBodyGetPosition(grabbedObject->body);
		const dReal* rot = dBodyGetQuaternion(grabbedObject->body);

		convert(objectTrans.position, pos);
		convert(objectTrans.orientation, rot);

// TODO: move this code to manipulationTerminationModel
		// ungrab object if distance is too high
// 		diff = userTrans.position - objectTrans.position;
// 		if (gmtl::length(diff) > maxDist)
// 		{
// 			printf("JointInteraction::update(): distance %f too high!\n", gmtl::length(diff));
// 			ungrab();
// 			return identityTransformation();
// 		} // if

// STEP 1: apply Torque
		// calculate Torque = angular offset from object orientation to current orientation
		rotation = userTrans.orientation;
		rotation *= invert(objectTrans.orientation);
		gmtl::set(AxAng, rotation);
		torque[0] = AxAng[0]*AxAng[1];  // rotAngle*rotX
		torque[1] = AxAng[0]*AxAng[2];  // rotAngle*rotY
		torque[2] = AxAng[0]*AxAng[3];  // rotAngle*rotZ

// TODO: move this code to manipulationTerminationModel
		// ungrab object if angular distance is too high
// 		if (gmtl::length(torque) > maxRotDist)
// 		{
// 			printf("JointInteraction::update(): rotation %f too high!\n", gmtl::length(torque));
// 			ungrab();
// 			return identityTransformation();
// 		} // if

		// create a Ball Joint to adjust orientation without changing the position
		dJointID universalJoint = dJointCreateBall(world, 0);
		dJointAttach(universalJoint, grabbedObject->body, 0);
		dJointSetBallAnchor(universalJoint, pos[0], pos[1], pos[2]);

//		printf("Torque = %f %f %f\n", torque[0], torque[1], torque[2]);
/***
 * old way --> is very instable due to high timestep!!!
 ***
 *		dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
 * // Half way to destination
 *		dWorldStep(world, 0.5);
 **/
		// take some simulation-steps for adjusting object's orientation
		for (int i=0; i < stepsPerFrame; i++)
		{
			dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
			dWorldStep(world, 1.0f/stepsPerFrame);
		} // for

		dJointDestroy(universalJoint);

// Reset speed and angular velocity to 0
		dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
		dBodySetAngularVel(grabbedObject->body, 0, 0, 0);

// STEP 2: apply force into wanted direction
		// calculate Force = vector from object position to current position
		force = (userTrans.position - objectTrans.position);

//		printf("Force = %f %f %f\n", force[0], force[1], force[2]);
/***
 * old way --> is very instable due to high timestep!!!
 ***
 *		dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
 * // Half way to destination
 *		dWorldStep(world, 0.5);
 **/
		// take some simulation-steps for adjusting object's position
		for (int i=0; i < stepsPerFrame; i++)
		{
			dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
			dWorldStep(world, 1.0f/stepsPerFrame);
		} // for

// Reset speed and angular velocity to 0
		dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
		dBodySetAngularVel(grabbedObject->body, 0, 0, 0);
	}

// Get position and Orientation from simulated object
	TransformationData newTransform = identityTransformation();
	const dReal* pos = dBodyGetPosition(grabbedObject->body);
	const dReal* rot = dBodyGetQuaternion(grabbedObject->body);

	convert(newTransform.orientation, rot);
	convert(newTransform.position, pos);

// Get position and Orientation from simulated objects
	std::map<int, ODEObject*>::iterator it = linkedObjMap.begin();
	ODEObject* object;
	TransformationData linkedObjectTrans = identityTransformation();
	while (it != linkedObjMap.end())
	{
		object = linkedObjMap[(*it).first];
		if (object != grabbedObject)
		{
			linkedObjectTrans = object->entity->getWorldTransformation();
			const dReal* pos1 = dBodyGetPosition(object->body);
			const dReal* rot1 = dBodyGetQuaternion(object->body);

			convert (linkedObjectTrans.orientation, rot1);
			convert (linkedObjectTrans.position, pos1);

			linkedObjPipes[it->first]->push_back(linkedObjectTrans);
// 			gmtl::set(AxAng, newRot);

/*			object->entity->setTranslation(pos1[0], pos1[1], pos1[2]);
			object->entity->setRotation(AxAng[1], AxAng[2], AxAng[3], AxAng[0]);*/
// 			assert(false);
// 			object->entity->setEnvironmentTransformation(linkedObjectTrans);
// 			object->entity->update();
		} // if
		++it;
	} // while

// Update angles in HingeJoints and check joint-constraints
	HingeJoint* hinge;
	for (int i=0; i < (int)attachedJoints.size(); i++)
	{
		hinge = dynamic_cast<HingeJoint*>(attachedJoints[i]);
		if (hinge)
			hinge->checkAngles();

		assert(attachedJoints[i]);
		attachedJoints[i]->checkConstraints();
	} // for

	TransformationData result;
	multiply(result, newTransform, deltaTrans);

	TransformationPipe* pipe = linkedObjPipes[grabbedObject->entity->getEnvironmentBasedId()];
	if (pipe)
		pipe->push_back(result);
	else
		printd(WARNING, "JointInteraction::update(): Could not find Pipe for manipulated Object!\n");

	return newTransform;
} // update
Example #8
0
void vmWishboneCar::buildWheelJoint(vmWheel *wnow, vmWishbone *snow, dReal shiftSign)
{
    // chassis pin joints
    const dReal *pos;
    snow->jChassisUp= dJointCreateHinge(world,0);
    dJointAttach(snow->jChassisUp,chassis.body,snow->uplink.body);
    dJointSetHingeAxis(snow->jChassisUp,1.0, 0.0, 0.0);
    pos= dBodyGetPosition(snow->uplink.body);
    dJointSetHingeAnchor(snow->jChassisUp,pos[0]
            ,pos[1]+0.5*shiftSign*snow->uplink.width, pos[2]);

    const dReal *pos1;
    snow->jChassisLow= dJointCreateHinge(world,0);
    dJointAttach(snow->jChassisLow, chassis.body, snow->lowlink.body);
    dJointSetHingeAxis(snow->jChassisLow,1.0, 0.0, 0.0);
    pos1= dBodyGetPosition(snow->lowlink.body);
    dJointSetHingeAnchor(snow->jChassisLow, pos1[0]
            , pos1[1]+0.5*shiftSign*snow->lowlink.width, pos1[2]);

    // rotor ball joint
    /*const dReal *p2;
    jRotorUp= dJointCreateBall(world,0);
    dJointAttach(jRotorUp, uplink.body, hlink.body);
    p2= dBodyGetPosition(uplink.body);
    dJointSetBallAnchor(jRotorUp, p2[0], p2[1]-0.5*uplink.width, p2[2]);

    const dReal *p3;
    jRotorLow= dJointCreateBall(world,0);
    dJointAttach(jRotorLow, lowlink.body,hlink.body);
    p3= dBodyGetPosition(lowlink.body);
    dJointSetBallAnchor(jRotorLow, p3[0], p3[1]-0.5*lowlink.width,p3[2]);*/

    const dReal *p2;
    snow->jRotorUp= dJointCreateHinge(world,0);
    dJointAttach(snow->jRotorUp, snow->uplink.body, snow->hlink.body);
    p2= dBodyGetPosition(snow->uplink.body);
    dJointSetHingeAnchor(snow->jRotorUp, p2[0]
            ,p2[1]-0.5*shiftSign*snow->uplink.width, p2[2]);
    dJointSetHingeAxis(snow->jRotorUp, 1.0, 0.0, 0.0);

    const dReal *p3;
    snow->jRotorLow= dJointCreateHinge(world,0);
    dJointAttach(snow->jRotorLow, snow->lowlink.body,snow->hlink.body);
    p3= dBodyGetPosition(snow->lowlink.body);
    dJointSetHingeAnchor(snow->jRotorLow, p3[0]
            ,p3[1]-0.5*shiftSign*snow->lowlink.width,p3[2]);
    dJointSetHingeAxis(snow->jRotorLow, 1.0, 0.0, 0.0);

    // rotor hinge joint
    const dReal *pw= dBodyGetPosition(wnow->body);
    snow->jRotorMid= dJointCreateHinge(world,0);
    dJointAttach(snow->jRotorMid, snow->hlink.body, wnow->body);
    dJointSetHingeAxis(snow->jRotorMid, 0.0,1.0,0.0);
    dJointSetHingeAnchor(snow->jRotorMid, pw[0],pw[1],pw[2]);


    // strut joint
    const dReal *ps1, *ps2;
    dReal angle= -shiftSign*strutAngle;

    ps1= dBodyGetPosition(snow->upstrut.body);
    ps2= dBodyGetPosition(snow->lowstrut.body);
    snow->jStrutUp= dJointCreateBall(world,0);
    dJointAttach(snow->jStrutUp, chassis.body, snow->upstrut.body);
    //dJointSetFixed(snow->jStrutUp);
    dJointSetBallAnchor(snow->jStrutUp, ps1[0]
            ,ps1[1]+0.5*shiftSign*snow->upstrut.width*fabs(sin(angle))
            ,ps1[2]+0.5*snow->upstrut.width*fabs(cos(angle)));

    snow->jStrutLow= dJointCreateBall(world,0);
    dJointAttach(snow->jStrutLow, snow->lowlink.body, snow->lowstrut.body);
    dJointSetBallAnchor(snow->jStrutLow, ps2[0]
            ,ps2[1]-0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle))
            ,ps2[2]-0.5*snow->lowstrut.width*fabs(cos(angle)));

    // struct sliding joint
    snow->jStrutMid= dJointCreateSlider(world,0);
    dJointAttach(snow->jStrutMid, snow->upstrut.body, snow->lowstrut.body);
    dJointSetSliderAxis(snow->jStrutMid, 0.0,shiftSign*fabs(sin(angle)),fabs(cos(angle)));

    // set joint feedback
    wnow->feedback= new dJointFeedback;
    dJointSetFeedback(snow->jStrutMid,wnow->feedback);

    // suspension slider
    /*snow->jLowSpring= dJointCreateLMotor(world,0);
    dJointAttach(snow->jLowSpring, chassis.body, snow->lowlink.body);
    dJointSetLMotorNumAxes(snow->jLowSpring, 1);
    dJointSetLMotorAxis(snow->jLowSpring,0,0, 0.0, 0.0, 1.0);*/


}
Example #9
0
void CBallJoint::UpdateTransform(const matrix44& Tfm)
{
	vector3 a = Tfm * Anchor;
	dJointSetBallAnchor(ODEJointID, a.x, a.y, a.z);
}
void MyDeformableObjectNode::createPhysicsBody(dWorldID odeWorld, dReal totalMass) {
    if (!odeWorld) {
        throw dcollide::NullPointerException("dWorldID odeWorld");
    }

    if (!mProxy->getShape()) {
        std::cerr << "WARNING: cannot create physics body for a shapeless Proxy."<<std::endl;
        return;
    }

    if (!mPhysicsObjects.empty()) {
        std::cerr << "WARNING: previously created a physics body for this node." << std::endl;
        return;
    }

    
    const std::vector<dcollide::Vertex*> vertices
        = mProxy->getShape()->getMesh()->getVertices();

    int verticesCount = vertices.size();


    // This object is physically equipped/enabled
    mUseODE = true;

    // Distribute the given mass to the whole body
    dReal vertexRadius = 0.5;
    dReal vertexMass = totalMass / verticesCount;

    // Create a physics mid point for the object when its a volume object
    dcollide::Vector3 meshCenter; 
    if (mProxy->getProxyType() & dcollide::PROXYTYPE_CLOSEDHULL) {
        mCenterPoint = new MyODETestAppGeom(this); 

        mCenterPoint->setMoveNodeOnBodyMoved(false);

        dMass* mass = new dMass();
        dBodyID body = dBodyCreate(odeWorld);

        dMassSetSphereTotal(mass, vertexMass, vertexRadius);

        //dBodySetMass(body, mass);
        dGeomSetBody((dGeomID)mCenterPoint, body);

        meshCenter = mProxy->getShape()->getMesh()->getMidPoint();
        dBodySetPosition(mCenterPoint->getBody(), meshCenter[0], meshCenter[1], meshCenter[2]);
    }


    // Create an physics body for each vertex of the mesh

    mPhysicsObjects.reserve(verticesCount);
    for (int index = 0; index < verticesCount; ++index) {

        mPhysicsObjects.push_back(
            new MyODEDeformableTestAppGeom(this, index)
        );

        dMass* mass = new dMass();
        dBodyID body = dBodyCreate(odeWorld);

        dMassSetSphereTotal(mass, vertexMass, vertexRadius);

        //dBodySetMass(body, mass);
        dGeomSetBody((dGeomID)mPhysicsObjects[index], body);


        // Adjust the ODE body position
        mPhysicsObjects[index]->setMoveNodeOnBodyMoved(false);

        const dcollide::Vector3& position = vertices[index]->getWorldPosition();
        dBodySetPosition(body, position[0], position[1], position[2]);

        mPhysicsObjects[index]->setMoveNodeOnBodyMoved(true);
    }


    // Connect the created physics bodies through joints

    // Create a joint groups (useful for static deformations)
    mJoints = dJointGroupCreate(0);

    // Set which avoids double creation
    std::set<dcollide::MultiMapElement<dcollide::Vertex, dJointID> > finished;
    std::set<dcollide::MultiMapElement<dcollide::Vertex, dJointID> >::iterator finished_pos;

    // Create the actual joints
    for (std::vector<dcollide::Vertex*>::const_iterator one = vertices.begin();
         one != vertices.end();
         ++one) {

        dcollide::Vertex* v1 = (*one);

        dBodyID body1 = mPhysicsObjects[v1->getVertexIndex()]->getBody();

        //const dReal* v1Pos = dBodyGetPosition(body1);
        //dcollide::debug() << "Body of vertex1 (worldPosition="<< v1->getWorldPosition()<<") is at (" << v1Pos[0] << ", " << v1Pos[1] << ", " << v1Pos[2] << ")";

        const std::list<dcollide::Vertex*>& list = (*one)->getAdjacentVertices();
        for (std::list<dcollide::Vertex*>::const_iterator two = list.begin();
             two != list.end();
             ++two) {

            dcollide::Vertex* v2 = (*two);
            dBodyID body2 = mPhysicsObjects[v2->getVertexIndex()]->getBody();

            finished_pos = finished.find(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v1, v2));
            
            if (finished_pos == finished.end()) {
                dcollide::Vector3 center  = (v1->getWorldPosition() + v2->getWorldPosition()) / 2;

                dJointID joint = dJointCreateBall(odeWorld, mJoints);
                //dJointID joint = dJointCreateFixed(odeWorld, mJoints);
                //dJointID joint = dJointCreateUniversal(odeWorld, mJoints);

                dJointAttach(joint, body1, body2);
                dJointSetBallAnchor(joint, center.getX(), center.getY(), center.getZ());
                //dJointSetFixed(joint);
/*
                dcollide::Vector3 axisOne  = (v1->getWorldPosition() - v2->getWorldPosition());
                dcollide::Vector3 axisTwo  = v1->getAdjacentTriangles().front()->getWorldCoordinatesNormalVector();

                dJointSetUniversalAnchor(joint, center.getX(), center.getY(), center.getZ());
                dJointSetUniversalAxis1(joint, axisOne.getX(), axisOne.getY(), axisOne.getZ());
                dJointSetUniversalAxis2(joint, axisTwo.getX(), axisTwo.getY(), axisTwo.getZ());
*/
                finished.insert(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v1, v2, joint));
                
/*
                // Create the so called bending joints
                std::list<dcollide::Vertex*>::const_iterator adjecent_pos;
                const std::list<dcollide::Vertex*>& farneigbours = v2->getAdjacentVertices();
                for (std::list<dcollide::Vertex*>::const_iterator neighbour = farneigbours.begin();
                     neighbour != farneigbours.end();
                     ++neighbour) {

                    dcollide::Vertex* v3 = (*neighbour);

                    adjecent_pos = find(list.begin(), list.end(), v3);
                    if (adjecent_pos == list.end()) {
                        // The current node is a far neighbour and thus should be connected

                        finished_pos = finished.find(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v2, v3));
            
                        // If it wasn't previously created -> create it!
                        if (finished_pos == finished.end()) {

                            dcollide::Vector3 center  = (v2->getWorldPosition() + v3->getWorldPosition()) / 2;

                            dJointID joint = dJointCreateBall(odeWorld, mJoints);

                            dJointAttach(joint, body1, body2);
                            dJointSetBallAnchor(joint, center.getX(), center.getY(), center.getZ());

                            finished.insert(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v2, v3, joint));
                        }
                    }
                }
*/
            }
        }

        // Connect all vertex "bodies" with the mid point
        if (mCenterPoint != 0) {
            dcollide::Vector3 center = (v1->getWorldPosition() + meshCenter) / 2;
            dcollide::Vector3 direction = v1->getWorldPosition() - meshCenter;

            dJointID joint = dJointCreateBall(odeWorld, mJoints);
            //dJointID joint = dJointCreateSlider(odeWorld, mJoints);

            dJointAttach(joint, body1, mCenterPoint->getBody());
            //dJointSetFixed(joint);
            
            dJointSetBallAnchor(joint, center.getX(), center.getY(), center.getZ());
            //dJointSetSliderAxis(joint, direction.getX(), direction.getY(), direction.getZ());
        }
    }
}
Example #11
0
void  makeRobot_Nleg() 
{ 
	for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		dReal torso_m = 50.0;                    // Mass of body
		//	torso_m[segment] = 10.0;
		
		dReal  l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments 
		
		//for four legs
		//		dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
		//																		{-cx1,-cx1,-cx1}};
		dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate)
			{0,0,0}};
		
		dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate)
			{-cy1,-cy1,-cy1}};
		dReal z[num_legs][num_links] = {                                  // Position of each link (z coordinate)
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} };
		
		dReal r[num_links]          =  { r1, r2, r3}; // radius of leg segment
		dReal length[num_links]     =  { l1, l2, l3}; // Length of leg segment
		dReal weight[num_links]     =  {l1m,l2m,l3m}; // Mass of leg segment
		
		//  //for one leg
		//  dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
		//  dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
		//  dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
		
		//for four legs
		dReal axis_x[num_legs][num_links];
		dReal axis_y[num_legs][num_links];
		dReal axis_z[num_legs][num_links];
		for(int i = 0; i < num_legs; ++i) {
			axis_x[i][0] = 0.0;
			axis_x[i][1] = 1.0;
			axis_x[i][2] = 0.0;
			
			axis_y[i][0] = 1.0;
			axis_y[i][1] = 0.0;
			axis_y[i][2] = 1.0;
			
			axis_z[i][0] = 0.0;
			axis_z[i][1] = 0.0;
			axis_z[i][2] = 0.0;
		}
		// For mation of the body
		dMass mass; 
		torso[segment].body  = dBodyCreate(world);
		dMassSetZero(&mass);
		dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
		dBodySetMass(torso[segment].body,&mass);
		torso[segment].geom = dCreateBox(space,lx, ly, lz); 
		dGeomSetBody(torso[segment].geom, torso[segment].body);
		dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ); 
		
		// Formation of leg
		dMatrix3 R;                          // Revolution queue
		dRFromAxisAndAngle(R,1,0,0,M_PI/2);  // 90 degrees to turn, parallel with the land
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) {
				THETA[segment][i][j] = 0;
				
				leg[segment][i][j].body = dBodyCreate(world);
				if (j == 0)
					dBodySetRotation(leg[segment][i][j].body,R);
				dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]);				
				dMassSetZero(&mass);
				dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
				dBodySetMass(leg[segment][i][j].body, &mass);
				//if(i==1 and j==2)                                         //to set the length of one leg differently
				//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
				//else
				leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
				dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body);
			}
		}
		
		// Formation of joints (and connecting them up)
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) { 
				leg[segment][i][j].joint = dJointCreateHinge(world, 0);
				if (j == 0){ 
					dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment
					dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees
					dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop,  .50*M_PI);
				}
				else
					dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body);
				
				dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]);
				dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
			} 
		}
	}
	
	
#ifdef USE_NLEG_ROBOT
	// link torsos
	for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		switch (hingeType) {
			case 1: //Hinge Joint, X axis  (back-breaker)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
				break;
			case 2: //Hinge Joint, Y axis (???)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 3: //Hinge Joint, Z axis (snake-like)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0);
				break;
			case 4: // Slider, Y axis  (??)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 5: // Slider, X axis  (extendo)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				break;
			case 6: //Universal Joint
				torso[segment].joint = dJointCreateUniversal(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0);
				dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0);				
				break;
			case 7: //Ball Joint
				torso[segment].joint = dJointCreateBall(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				break;
			case 8:  // Fixed
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees
				dJointSetHingeParam(torso[segment].joint, dParamHiStop,  0.00*M_PI);
				break;
			default:
				assert(false); // not a valid hinge type
				break;
		}
	}
#endif
}
Example #12
0
void odCable::setupBody(dWorldID *world, dSpaceID space){
  int i;
  double pi=4.*atan(1.);
  double segLen, dL, x;
  double dd, s, rs;
  dVector3 OP;
  odPoint loc1, loc2, d, P, c;
  dMass *m;
  dGeomID geometry[nSegments];
//  dQuaternion align;
  dReal align[4];
  
  printf("Setting up cable model\n");
  
  if (body1!=NULL){
    loc1=body1->globalPosition(pos1);
  }else{
    loc1=pos1;
  }
  if (body2!=NULL){
    loc2=body2->globalPosition(pos2);
  }else{
    loc2=pos2;
  }
  
  d=loc2-loc1;
  dL=d.mag()/(double)nSegments;
  segLen=length/(double)nSegments;
  printf("Distance = %lf m\n",d.mag());
  printf("Segment length = %lf m\n",segLen);
  
  printf("Allocating memory\n");
  element = new dBodyID[nSegments];
  joint = new dJointID[nSegments-1];
  
  printf("Creating elements\n");
  for (i=0;i<nSegments;i++){
    element[i] = dBodyCreate (*world);
    dBodySetDamping(element[i], linearDamping, angularDamping);
    m=new dMass;
    dMassSetBoxTotal(m, weight*segLen, segLen, diameter, diameter);
    dMassAdjust (m, weight*segLen);
    dBodySetMass (element[i], m);
    x=((double)i+.5)*segLen;
    dBodySetPosition(element[i], x, 0, 0);
//    geometry[i] = dCreateBox(space, length, diameter, diameter);
//    dGeomSetBody(geometry[i], element[i]);
  }
  printf("Creating internal joints\n");
  for (i=0;i<nSegments-1;i++){
    joint[i] = dJointCreateBall(*world,0);
    dJointAttach (joint[i],element[i],element[i+1]);
    x = ((double)i+1.)*segLen;
    dJointSetBallAnchor (joint[i],x,0,0);
  }
/*
  c = d.crossProduct(odPoint(0,0,1)); 
  dd = d.dotProduct_natural(odPoint(0,0,1)); 
  s = sqrt((1.0 + dd)*2.0); 
  rs = 1.0/s;
  align[0]=s*0.5; align[1]=c.x*rs; align[2]=c.y*rs; align[3]=c.z*rs;

  printf("Moving segments to world positions...\n");
  for (i=0;i<nSegments;i++){
    P=loc1+d*((double)i)/(nSegments-1);
    if (i==0) P.x+=segLen/2.;
    if (i==nSegments-1) P.x-=segLen/2.;
    dBodySetPosition(element[i], P.x, P.y, P.z);
//    dBodySetQuaternion(element[i], align);
  }
*/  
  printf("Creating end joint 1\n");
  end1 = dJointCreateBall(*world,0);
  if (body1!=NULL){
    dJointAttach(end1, body1->odeBody, element[0]);
  }else{
    dJointAttach(end1, 0, element[0]);
  }
//  dJointSetBallAnchor (end1, loc1.x, loc1.y, loc1.z);
  dJointSetBallAnchor (end1, 0, 0, 0);
  end1Feedback = new dJointFeedback;
  dJointSetFeedback (end1, end1Feedback);

/*  
  printf("Creating end joint 2\n");  
  end2 = dJointCreateBall(*world,0);
  if (body2!=NULL){
    dJointAttach(end2, body2->odeBody, element[nSegments-1]);
  }else{
    dJointAttach(end2, 0, element[nSegments-1]);
  }
//  dJointSetBallAnchor (end2, loc2.x, loc2.y, loc2.z);
  dJointSetBallAnchor (end2, nSegments*segLen, 0, 0);
  */
  end2Feedback = new dJointFeedback;
  dJointSetFeedback (joint[0], end2Feedback);
}
Example #13
0
void CPHCapture::PullingUpdate()
{
    if(!m_taget_element->isActive()||inl_ph_world().Device().dwTimeGlobal-m_time_start>m_capture_time)
    {
        Release();
        return;
    }

    Fvector dir;
    Fvector capture_bone_position;
    //CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject());
    capture_bone_position.set(m_capture_bone->mTransform.c);
    m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position);
    m_taget_element->GetGlobalPositionDynamic(&dir);
    dir.sub(capture_bone_position,dir);
    float dist=dir.magnitude();
    if(dist>m_pull_distance)
    {
        Release();
        return;
    }
    dir.mul(1.f/dist);
    if(dist<m_capture_distance)
    {
        m_back_force=0.f;

        m_joint=dJointCreateBall(0,0);
        m_island.AddJoint(m_joint);
        m_ajoint=dJointCreateAMotor(0,0);
        m_island.AddJoint(m_ajoint);
        dJointSetAMotorMode (m_ajoint, dAMotorEuler);
        dJointSetAMotorNumAxes (m_ajoint, 3);

        CreateBody();
        dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
        VERIFY( smart_cast<CPHElement*>(m_taget_element) );
        CPHElement	* e = static_cast<CPHElement*>(m_taget_element);
        dJointAttach(m_joint,m_body,e->get_body());
        dJointAttach(m_ajoint,m_body,e->get_body());
        dJointSetFeedback (m_joint, &m_joint_feedback);
        dJointSetFeedback (m_ajoint, &m_joint_feedback);
        dJointSetBallAnchor(m_joint,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);


        dJointSetAMotorAxis (m_ajoint, 0, 1, dir.x, dir.y, dir.z);

        if(dir.x>EPS)
        {
            if(dir.y>EPS)
            {
                float mag=dir.x*dir.x+dir.y*dir.y;
                dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.y/mag, dir.x/mag, 0.f);
            }
            else if(dir.z>EPS)
            {
                float mag=dir.x*dir.x+dir.z*dir.z;
                dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.z/mag,0.f,dir.x/mag);
            }
            else
            {
                dJointSetAMotorAxis (m_ajoint, 2, 2, 1.f,0.f,0.f);
            }
        }
        else
        {
            if(dir.y>EPS)
            {

                if(dir.z>EPS)
                {
                    float mag=dir.y*dir.y+dir.z*dir.z;
                    dJointSetAMotorAxis (m_ajoint, 2, 2,0.f,-dir.z/mag,dir.y/mag);
                }
                else
                {
                    dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,1.f,0.f);
                }
            }
            else
            {
                dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,0.f,1.f);
            }
        }
        //float hi=-M_PI/2.f,lo=-hi;
        //dJointSetAMotorParam(m_ajoint,dParamLoStop ,lo);
        //dJointSetAMotorParam(m_ajoint,dParamHiStop ,hi);
        //dJointSetAMotorParam(m_ajoint,dParamLoStop2 ,lo);
        //dJointSetAMotorParam(m_ajoint,dParamHiStop2 ,hi);
        //dJointSetAMotorParam(m_ajoint,dParamLoStop3 ,lo);
        //dJointSetAMotorParam(m_ajoint,dParamHiStop3 ,hi);


        dJointSetAMotorParam(m_ajoint,dParamFMax ,m_capture_force*0.2f);
        dJointSetAMotorParam(m_ajoint,dParamVel  ,0.f);

        dJointSetAMotorParam(m_ajoint,dParamFMax2 ,m_capture_force*0.2f);
        dJointSetAMotorParam(m_ajoint,dParamVel2  ,0.f);

        dJointSetAMotorParam(m_ajoint,dParamFMax3 ,m_capture_force*0.2f);
        dJointSetAMotorParam(m_ajoint,dParamVel3  ,0.f);


///////////////////////////////////
        float sf=0.1f,df=10.f;

        float erp=ERP(world_spring*sf,world_damping*df);
        float cfm=CFM(world_spring*sf,world_damping*df);
        dJointSetAMotorParam(m_ajoint,dParamStopERP ,erp);
        dJointSetAMotorParam(m_ajoint,dParamStopCFM ,cfm);

        dJointSetAMotorParam(m_ajoint,dParamStopERP2 ,erp);
        dJointSetAMotorParam(m_ajoint,dParamStopCFM2 ,cfm);

        dJointSetAMotorParam(m_ajoint,dParamStopERP3 ,erp);
        dJointSetAMotorParam(m_ajoint,dParamStopCFM3 ,cfm);
        /////////////////////////////////////////////////////////////////////
        ///dJointSetAMotorParam(m_joint1,dParamFudgeFactor ,0.1f);
        //dJointSetAMotorParam(m_joint1,dParamFudgeFactor2 ,0.1f);
        //dJointSetAMotorParam(m_joint1,dParamFudgeFactor3 ,0.1f);
        /////////////////////////////////////////////////////////////////////////////
        sf=0.1f,df=10.f;
        erp=ERP(world_spring*sf,world_damping*df);
        cfm=CFM(world_spring*sf,world_damping*df);
        dJointSetAMotorParam(m_ajoint,dParamCFM ,cfm);
        dJointSetAMotorParam(m_ajoint,dParamCFM2 ,cfm);
        dJointSetAMotorParam(m_ajoint,dParamCFM3 ,cfm);


///////////////////////////

        //dJointSetAMotorParam(m_ajoint,dParamLoStop ,0.f);
        //dJointSetAMotorParam(m_ajoint,dParamHiStop ,0.f);
        m_taget_element->set_LinearVel ( Fvector().set( 0 ,0, 0 ) );
        m_taget_element->set_AngularVel( Fvector().set( 0 ,0, 0 ) );


        m_taget_element->set_DynamicLimits();
        //m_taget_object->PPhysicsShell()->set_JointResistance()
        e_state=cstCaptured;
        return;
    }
    m_taget_element->applyForce(dir,m_pull_force);
}
Example #14
0
void createTest()
{
  int i,j;
  if (world) dWorldDestroy (world);

  world = dWorldCreate();

  // create random bodies
  for (i=0; i<NUM; i++) {
    // create bodies at random position and orientation
    body[i] = dBodyCreate (world);
    // dBodySetPosition (body[i],dRandReal()*2-1,dRandReal()*2-1,
    // 		      dRandReal()*2+RADIUS);
    dBodySetPosition (body[i],dRandReal()*SIDE-1,dRandReal()*SIDE-1,
    		      dRandReal()*SIDE+RADIUS);

    dReal q[4];
    for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
    dBodySetQuaternion (body[i],q);

    // set random velocity
    dBodySetLinearVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
		       dRandReal()*2-1);
    dBodySetAngularVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
			dRandReal()*2-1);

    // set random mass (random diagonal mass rotated by a random amount)
    dMass m;
    dMatrix3 R;
    dMassSetBox (&m,1,dRandReal()+0.1,dRandReal()+0.1,dRandReal()+0.1);
    dMassAdjust (&m,dRandReal()+1);
    for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
    dQtoR (q,R);
    dMassRotate (&m,R);
    dBodySetMass (body[i],&m);
  }

  // create ball-n-socket joints at random positions, linking random bodies
  // (but make sure not to link the same pair of bodies twice)
  for (i=0; i<NUM*NUM; i++) linked[i] = 0;
  for (i=0; i<NUMJ; i++) {
    int b1,b2;
    do {
      b1 = dRandInt (NUM);
      b2 = dRandInt (NUM);
    } while (linked[b1*NUM + b2] || b1==b2);
    linked[b1*NUM + b2] = 1;
    linked[b2*NUM + b1] = 1;
    joint[i] = dJointCreateBall (world,0);
    dJointAttach (joint[i],body[b1],body[b2]);
    dJointSetBallAnchor (joint[i],dRandReal()*2-1,
			 dRandReal()*2-1,dRandReal()*2+RADIUS);
  }

  for (i=0; i<NUM; i++) {
    // move bodies a bit to get some joint error
    const dReal *pos = dBodyGetPosition (body[i]); 
    dBodySetPosition (body[i],pos[0]+dRandReal()*0.2-0.1,
		      pos[1]+dRandReal()*0.2-0.1,pos[2]+dRandReal()*0.2-0.1);
  }
}
// called by Webots at the beginning of the simulation
void webots_physics_init(dWorldID w, dSpaceID s, dJointGroupID j) {

  int i;

  // store global objects for later use
  world = w;
  space = s;
  contact_joint_group = j;

  // get floor geometry
  floor_geom = getGeom(floor_name);
  if (!floor_geom)
    return;

  // get foot geometry and body id's
  for (i = 0; i < N_FEET; i++) {
    foot_geom[i] = getGeom(foot_name[i]);
    if (!foot_geom[i])
      return;
    foot_body[i] = dGeomGetBody(foot_geom[i]);
    if (!foot_body[i])
      return;
  }

  // create universal joints for linear actuators
  for (i = 0; i < 10; i++) {
    dBodyID upper_piston = getBody(upper_piston_name[i]);
    dBodyID lower_piston = getBody(lower_piston_name[i]);
    dBodyID upper_link = getBody(upper_link_name[i]);
    dBodyID lower_link = getBody(lower_link_name[i]);
    if (!upper_piston || !lower_piston || !upper_link || !lower_link)
      return;

    // create a ball and socket joint (3 DOFs) to attach the lower piston body to the lower link 
    // we don't need a universal joint here, because the piston's passive rotation is prevented
    // by the universal joint at its upper end.
    dJointID lower_balljoint = dJointCreateBall(world, 0);
    dJointAttach(lower_balljoint, lower_piston, lower_link);

    // transform attachement point from local to global coordinate system
    // warning: this is a hard-coded translation 
    dVector3 lower_ball;
    dBodyGetRelPointPos(lower_piston, 0, 0, -0.075, lower_ball);

    // set attachement point (anchor)
    dJointSetBallAnchor(lower_balljoint, lower_ball[0], lower_ball[1], lower_ball[2]);
    
    // create a universal joint (2 DOFs) to attach upper piston body to upper link
    // we need to use a universal joint to prevent the piston from passively rotating around its long axis
    dJointID upper_ujoint = dJointCreateUniversal(world, 0);
    dJointAttach(upper_ujoint, upper_piston, upper_link);

    // transform attachement point from local to global coordinate system
    // warning: this is a hard-coded translation 
    dVector3 upper_ball;
    dBodyGetRelPointPos(upper_piston, 0, 0, 0, upper_ball);

    // set attachement point (anchor)
    dJointSetUniversalAnchor(upper_ujoint, upper_ball[0], upper_ball[1], upper_ball[2]);

    // set the universal joint axes
    dVector3 upper_xaxis;
    dVector3 upper_yaxis;
    dBodyVectorToWorld(upper_piston, 1, 0, 0, upper_xaxis);
    dBodyVectorToWorld(upper_piston, 0, 1, 0, upper_yaxis);
    dJointSetUniversalAxis1(upper_ujoint, upper_xaxis[0], upper_xaxis[1], upper_xaxis[2]);
    dJointSetUniversalAxis2(upper_ujoint, upper_yaxis[0], upper_yaxis[1], upper_yaxis[2]);

  }
}
Example #16
0
void reset_test()
{
  int i;
  dMass m,anchor_m;
  dReal q[NUM][3], pm[NUM];	// particle positions and masses
  dReal pos1[3] = {1,0,1};	// point of reference (POR)
  dReal pos2[3] = {-1,0,1};	// point of reference (POR)

  // make random particle positions (relative to POR) and masses
  for (i=0; i<NUM; i++) {
    pm[i] = dRandReal()+0.1;
    q[i][0] = dRandReal()-0.5;
    q[i][1] = dRandReal()-0.5;
    q[i][2] = dRandReal()-0.5;
  }

  // adjust particle positions so centor of mass = POR
  computeMassParams (&m,q,pm);
  for (i=0; i<NUM; i++) {
    q[i][0] -= m.c[0];
    q[i][1] -= m.c[1];
    q[i][2] -= m.c[2];
  }

  if (world) dWorldDestroy (world);
  world = dWorldCreate();

  anchor_body = dBodyCreate (world);
  dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
  dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
  dMassAdjust (&anchor_m,0.1);
  dBodySetMass (anchor_body,&anchor_m);

  for (i=0; i<NUM; i++) {
    particle[i] = dBodyCreate (world);
    dBodySetPosition (particle[i],
		      pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
    dMassSetBox (&m,1,SIDE,SIDE,SIDE);
    dMassAdjust (&m,pm[i]);
    dBodySetMass (particle[i],&m);
  }

  for (i=0; i < NUM; i++) {
    particle_joint[i] = dJointCreateBall (world,0);
    dJointAttach (particle_joint[i],anchor_body,particle[i]);
    const dReal *p = dBodyGetPosition (particle[i]);
    dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
  }

  // make test_body with the same mass and inertia of the anchor_body plus
  // all the particles

  test_body = dBodyCreate (world);
  dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
  computeMassParams (&m,q,pm);
  m.mass += anchor_m.mass;
  for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
  dBodySetMass (test_body,&m);

  // rotate the test and anchor bodies by a random amount
  dQuaternion qrot;
  for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
  dNormalize4 (qrot);
  dBodySetQuaternion (anchor_body,qrot);
  dBodySetQuaternion (test_body,qrot);
  dMatrix3 R;
  dQtoR (qrot,R);
  for (i=0; i<NUM; i++) {
    dVector3 v;
    dMultiply0 (v,R,&q[i][0],3,3,1);
    dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
  }

  // set random torque
  for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;


  iteration=0;
}
Example #17
0
void CODEBallJoint::setParams()
{
	double anchorPos[3];
	mpBodyAnchors[0]->getPosition(anchorPos);
	dJointSetBallAnchor(mID, anchorPos[0], anchorPos[1], anchorPos[2]);
}