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 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 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;
}
// 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 CODEBallJoint::create(dWorld& world, dJointGroupID groupID)
{
mID = dJointCreateBall(world, groupID);
}
// NOTE: it is important that rigid bodies are added
// (happens in CJoint::Attach()) before joint transforms are set!!!
void CBallJoint::Attach(dWorldID WorldID, dJointGroupID GroupID, const matrix44& ParentTfm)
{
ODEJointID = dJointCreateBall(WorldID, GroupID);
CJoint::Attach(WorldID, GroupID, ParentTfm);
UpdateTransform(ParentTfm);
}
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);
}
}
/**
* 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
/**
Update the marker positions and velocities.
If the data model is paused, then we still
set the position of the markers so that they
can be displayed. However, the velocity is
zero. If the model is not paused, we set the
marker location and set its velocity. Then
we advance the current marker frame forward.
If the current marker frame is not visible,
the marker goes to the origin (or below the
plane).
If either frame is not visible, we don't
attach the joint.
*/
void MarkerData::step()
{
// Set the markers to hold the
// current frame
// Get the current frame
// Get the next frame
if (!use_virtual_markers) {
data.readFrame(current_frame,c_frame);
data.readFrame(current_frame+frames_per_step,n_frame);
} else {
if (virtual_point>=0 && virtual_point<shadow_data.size()) {
c_frame = shadow_data[virtual_point];
if (virtual_point==shadow_data.size()-1) {
n_frame = shadow_data[virtual_point];
} else {
n_frame = shadow_data[virtual_point+1];
}
} else {
c_frame = n_frame = shadow_data.last();
}
}
dJointGroupEmpty(joint_group);
if (paused && !single_step) {
for (int ii=0;ii<marker_count;++ii) {
float xx,yy,zz;
xx = c_frame->data[ii].point[0]*.001;
yy = c_frame->data[ii].point[1]*.001;
zz = c_frame->data[ii].point[2]*.001;
if (c_frame->data[ii].point[3]<0) zz=-1;
dBodySetLinearVel(body[ii],0,0,0);
dBodySetPosition(body[ii],xx,yy,zz);
}
} else {
for (int ii=0;ii<marker_count;++ii) {
float xx,yy,zz;
xx = c_frame->data[ii].point[0]*.001;
yy = c_frame->data[ii].point[1]*.001;
zz = c_frame->data[ii].point[2]*.001;
if (c_frame->data[ii].point[3]<0) zz=-1;
float dx,dy,dz;
dx = n_frame->data[ii].point[0]*.001 - xx;
dy = n_frame->data[ii].point[1]*.001 - yy ;
dz = n_frame->data[ii].point[2]*.001 - zz;
dBodySetLinearVel(body[ii],dx/time_per_step,dy/time_per_step,dz/time_per_step);
dBodySetPosition(body[ii],xx,yy,zz);
}
//currentFrame+=framesPerStep;
setFrame(current_frame+frames_per_step);
if (use_virtual_markers) virtual_point+=1;
single_step = false;
}
for (int ii=0;ii<marker_count;++ii) {
int bID = body_pointer->marker_to_body[ii].id;
if (try_link[ii] && (bID>=0) &&
(c_frame->data[ii].point[3]>0) &&
(n_frame->data[ii].point[3]>0))
{
joint[ii]=dJointCreateBall(world,joint_group);
dJointAttach(joint[ii],body[ii],body_pointer->body_segments[bID]);
dJointSetBallAnchor1Rel(joint[ii],0,0,0);
dJointSetBallAnchor2Rel(joint[ii],
body_pointer->marker_to_body[ii].position[0],
body_pointer->marker_to_body[ii].position[1],
body_pointer->marker_to_body[ii].position[2]);
dJointSetBallParam(joint[ii],dParamCFM,.0001);
dJointSetBallParam(joint[ii],dParamERP,.2);
}
}
if (use_virtual_markers){
c_frame = old_c_frame;
n_frame = old_n_frame;
}
}
