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;
}
}
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);
}
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;
}
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;
}
/**
* 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;
}
/**
* 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
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);*/
}
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());
}
}
}
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
}
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);
}
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);
}
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]);
}
}
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;
}
void CODEBallJoint::setParams()
{
double anchorPos[3];
mpBodyAnchors[0]->getPosition(anchorPos);
dJointSetBallAnchor(mID, anchorPos[0], anchorPos[1], anchorPos[2]);
}