void CHolonomicPathNode::getAllValues(C3Vector& pos, C4Vector& orient) { pos.clear(); orient.setIdentity(); if (_nodeType == sim_holonomicpathplanning_xy) { pos(0) = values[0]; pos(1) = values[1]; } if (_nodeType == sim_holonomicpathplanning_xg) { pos(0) = values[0]; orient = _rotAxisRot * (C4Vector(C3Vector(0.0f, 0.0f, values[1])) * _rotAxisRotInv); } if (_nodeType == sim_holonomicpathplanning_xyz) { pos(0) = values[0]; pos(1) = values[1]; pos(2) = values[2]; } if (_nodeType == sim_holonomicpathplanning_xyg) { pos(0) = values[0]; pos(1) = values[1]; orient = _rotAxisRot * (C4Vector(C3Vector(0.0f, 0.0f, values[2])) * _rotAxisRotInv); } if (_nodeType == sim_holonomicpathplanning_abg) { orient(0) = values[0]; orient(1) = values[1]; orient(2) = values[2]; orient(3) = values[3]; } if (_nodeType == sim_holonomicpathplanning_xyzg) { pos(0) = values[0]; pos(1) = values[1]; pos(2) = values[2]; orient = _rotAxisRot * (C4Vector(C3Vector(0.0f, 0.0f, values[3])) * _rotAxisRotInv); } if (_nodeType == sim_holonomicpathplanning_xabg) { pos(0) = values[0]; orient(0) = values[1]; orient(1) = values[2]; orient(2) = values[3]; orient(3) = values[4]; } if (_nodeType == sim_holonomicpathplanning_xyabg) { pos(0) = values[0]; pos(1) = values[1]; orient(0) = values[2]; orient(1) = values[3]; orient(2) = values[4]; orient(3) = values[5]; } if (_nodeType == sim_holonomicpathplanning_xyzabg) { pos(0) = values[0]; pos(1) = values[1]; pos(2) = values[2]; orient(0) = values[3]; orient(1) = values[4]; orient(2) = values[5]; orient(3) = values[6]; } }
C7Vector CIKGraphJoint::getDownToTopTransformation() { C7Vector retVal; retVal.setIdentity(); if (jointType==IK_GRAPH_SPHERICAL_JOINT_TYPE) retVal.Q=sphericalTransformation; else if (jointType==IK_GRAPH_REVOLUTE_JOINT_TYPE) retVal.Q.setAngleAndAxis(parameter,C3Vector(0.0f,0.0f,1.0f)); else if (jointType==IK_GRAPH_PRISMATIC_JOINT_TYPE) retVal.X(2)=parameter; return(retVal); }
float CIKJoint::getValueOverLimitation(bool deactivate) { if (!active) { // The case (!active)&&spherical&&topSpherical&&(!sphericalUp) should not return here! if (!spherical) return(-1.0f); // No limitation violation if (!topSpherical) return(-1.0f); if (sphericalUp) return(-1.0f); } if (spherical) { if (!topSpherical) return(-1.0f); if (range>179.9f*degToRad) return(-1.0f); if (sphericalUp) { // Bottom-up if (!parent->active) return(-1.0f); // Was already deactivated C4Vector rotYm90(C3Vector(0.0f,-piValue/2.0f,0.0f)); C4Vector tr0(C3Vector(parent->parent->tempParameter,parent->tempParameter,0.0f)); C4Vector tr(C3Vector(parent->parent->tempParameter+parent->parent->probableDeltaValue,parent->tempParameter+parent->probableDeltaValue,0.0f)); C4Vector sphericalTr(parent->parent->transformation.Q*rotYm90); tr0=sphericalTr*tr0; tr=sphericalTr*tr; C3X3Matrix m0(tr0.getMatrix()); C3X3Matrix m(tr.getMatrix()); C3Vector z0(m0(0,2),m0(1,2),m0(2,2)); C3Vector z(m(0,2),m(1,2),m(2,2)); C3Vector zVertical(0.0f,0.0f,1.0f); C4Vector zVerticalToZ0(zVertical,z0); C4Vector zVerticalToZ(zVertical,z); C4Vector angleAndAxis0(zVerticalToZ0.getAngleAndAxis()); C4Vector angleAndAxis(zVerticalToZ.getAngleAndAxis()); float p=angleAndAxis(0); float deltaV=p-angleAndAxis0(0); float tolerance=range*0.0001f; bool respected=true; if (p>range+tolerance) { p=range; respected=false; } if (!respected) { float retVal=fabs((p-angleAndAxis0(0)+deltaV)/deltaV); if (deactivate) { tr.setAngleAndAxis(p,C3Vector(angleAndAxis(1),angleAndAxis(2),angleAndAxis(3))); tr=sphericalTr.getInverse()*tr; C3Vector euler(tr.getEulerAngles()); parent->tempParameter=euler(1); parent->active=false; parent->copyStateToAvatarKids(); parent->parent->tempParameter=euler(0); parent->parent->active=false; parent->parent->copyStateToAvatarKids(); parent->parent->parent->active=true; parent->parent->parent->copyStateToAvatarKids(); } return(retVal); } } else { // Top-down if (active) return(-1.0f); // Was already deactivated C4Vector rotY180(C3Vector(0.0f,piValue,0.0f)); C4Vector rotXm90(C3Vector(-piValue/2.0f,0.0f,0.0f)); C4Vector rotZ90(C3Vector(0.0f,0.0f,piValue/2.0f)); C4Vector tr0(C3Vector(parent->tempParameter,parent->parent->tempParameter,0.0f)); C4Vector tr(C3Vector(parent->tempParameter+parent->probableDeltaValue,parent->parent->tempParameter+parent->parent->probableDeltaValue,0.0f)); C4Vector sphericalTr((rotZ90*rotXm90*transformation.Q*rotY180).getInverse()); tr0=sphericalTr*tr0; tr=sphericalTr*tr; C3X3Matrix m0(tr0.getMatrix()); C3X3Matrix m(tr.getMatrix()); C3Vector z0(m0(0,2),m0(1,2),m0(2,2)); C3Vector z(m(0,2),m(1,2),m(2,2)); C3Vector zVertical(0.0f,0.0f,1.0f); C4Vector zVerticalToZ0(zVertical,z0); C4Vector zVerticalToZ(zVertical,z); C4Vector angleAndAxis0(zVerticalToZ0.getAngleAndAxis()); C4Vector angleAndAxis(zVerticalToZ.getAngleAndAxis()); float p=angleAndAxis(0); float deltaV=p-angleAndAxis0(0); float tolerance=range*0.0001f; bool respected=true; if (p>range+tolerance) { p=range; respected=false; } if (!respected) { float retVal=fabs((p-angleAndAxis0(0)+deltaV)/deltaV); if (deactivate) { tr.setAngleAndAxis(p,C3Vector(angleAndAxis(1),angleAndAxis(2),angleAndAxis(3))); tr=sphericalTr.getInverse()*tr; C3Vector euler(tr.getEulerAngles()); active=true; copyStateToAvatarKids(); parent->tempParameter=euler(0); parent->active=false; parent->copyStateToAvatarKids(); parent->parent->tempParameter=euler(1); parent->parent->active=false; parent->parent->copyStateToAvatarKids(); } return(retVal); } } return(-1.0f); } else { float p=tempParameter+probableDeltaValue; float tolerance=range*0.0001f; bool respected=true; if (revolute) { if ( (range<359.9f*degToRad)&&(!cyclic) ) { if (p<minValue-tolerance) { p=minValue; respected=false; } else if (p>minValue+range+tolerance) { p=minValue+range; respected=false; } } } else { if (p<minValue-tolerance) { p=minValue; respected=false; } else if (p>minValue+range+tolerance) { p=minValue+range; respected=false; } } if (!respected) { float retVal=fabs((p-tempParameter+probableDeltaValue)/probableDeltaValue); if (deactivate) { tempParameter=p; active=false; copyStateToAvatarKids(); } return(retVal); } return(-1.0f); // Respected } }
void executeRenderCommands(bool windowed,int message,void* data) { if (message==sim_message_eventcallback_extrenderer_start) { // Collect camera and environment data from V-REP: void** valPtr=(void**)data; resolutionX=((int*)valPtr[0])[0]; resolutionY=((int*)valPtr[1])[0]; float* backgroundColor=((float*)valPtr[2]); float viewAngle=((float*)valPtr[8])[0]; perspectiveOperation=(((int*)valPtr[5])[0]==0); nearClippingPlane=((float*)valPtr[9])[0]; farClippingPlane=((float*)valPtr[10])[0]; float* amb=(float*)valPtr[11]; C7Vector cameraTranformation(C4Vector((float*)valPtr[4]),C3Vector((float*)valPtr[3])); C4X4Matrix m4(cameraTranformation.getMatrix()); float* fogBackgroundColor=(float*)valPtr[12]; int fogType=((int*)valPtr[13])[0]; float fogStart=((float*)valPtr[14])[0]; float fogEnd=((float*)valPtr[15])[0]; float fogDensity=((float*)valPtr[16])[0]; bool fogEnabled=((bool*)valPtr[17])[0]; float orthoViewSize=((float*)valPtr[18])[0]; visionSensorOrCameraId=((int*)valPtr[19])[0]; int posX=0; int posY=0; if ((valPtr[20]!=NULL)&&(valPtr[21]!=NULL)) { posX=((int*)valPtr[20])[0]; posY=((int*)valPtr[21])[0]; } float fogDistance=((float*)valPtr[22])[0]; // pov-ray float fogTransp=((float*)valPtr[23])[0]; // pov-ray bool povFocalBlurEnabled=((bool*)valPtr[24])[0]; // pov-ray float povFocalDistance=((float*)valPtr[25])[0]; // pov-ray float povAperture=((float*)valPtr[26])[0]; // pov-ray int povBlurSamples=((int*)valPtr[27])[0]; // pov-ray COpenglBase* oglItem=NULL; if (windowed&&_simulationRunning) { COpenglWidget* oglWidget=getWidget(visionSensorOrCameraId); if (oglWidget==NULL) { oglWidget=new COpenglWidget(visionSensorOrCameraId); oglWidgets.push_back(oglWidget); oglWidget->initGL(); oglWidget->showAtGivenSizeAndPos(resolutionX,resolutionY,posX,posY); } // the window size can change, we return those values: oglWidget->getWindowResolution(resolutionX,resolutionY); ((int*)valPtr[0])[0]=resolutionX; ((int*)valPtr[1])[0]=resolutionY; oglItem=oglWidget; } else { // non-windowed COpenglOffscreen* oglOffscreen=getOffscreen(visionSensorOrCameraId); if (oglOffscreen!=NULL) { if (!oglOffscreen->isResolutionSame(resolutionX,resolutionY)) { removeOffscreen(visionSensorOrCameraId); oglOffscreen=NULL; } } if (oglOffscreen==NULL) { oglOffscreen=new COpenglOffscreen(visionSensorOrCameraId,resolutionX,resolutionY); oglOffscreens.push_back(oglOffscreen); oglOffscreen->initGL(); } oglItem=oglOffscreen; } if (oglItem!=NULL) { oglItem->makeContextCurrent(); oglItem->clearBuffers(viewAngle,orthoViewSize,nearClippingPlane,farClippingPlane,perspectiveOperation,backgroundColor); if (meshContainer==NULL) { meshContainer=new COcMeshContainer(); textureContainer=new COcTextureContainer(); } // The following instructions have the same effect as gluLookAt() m4.inverse(); m4.rotateAroundY(3.14159265359f); float m4_[4][4]; m4.copyTo(m4_); #define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;} float temp; SWAP(m4_[0][1],m4_[1][0]); SWAP(m4_[0][2],m4_[2][0]); SWAP(m4_[0][3],m4_[3][0]); SWAP(m4_[1][2],m4_[2][1]); SWAP(m4_[1][3],m4_[3][1]); SWAP(m4_[2][3],m4_[3][2]); #undef SWAP glLoadMatrixf((float*)m4_); GLfloat ambient[4]={amb[0],amb[1],amb[2],1.0f}; glLightModelfv(GL_LIGHT_MODEL_AMBIENT,ambient); if (fogEnabled) { float fog_color[4]={fogBackgroundColor[0],fogBackgroundColor[1],fogBackgroundColor[2],1.0f}; GLenum fogTypeEnum[3]={GL_LINEAR,GL_EXP,GL_EXP2}; glFogfv(GL_FOG_COLOR,fog_color); glFogi(GL_FOG_MODE,fogTypeEnum[fogType]); glFogf(GL_FOG_START,fogStart); glFogf(GL_FOG_END,fogEnd); glFogf(GL_FOG_DENSITY,fogDensity); glEnable(GL_FOG); } activeLightCounter=0; } } if (message==sim_message_eventcallback_extrenderer_light) { // Collect light data from V-REP (one light at a time): void** valPtr=(void**)data; int lightType=((int*)valPtr[0])[0]; float cutoffAngle=((float*)valPtr[1])[0]; int spotExponent=((int*)valPtr[2])[0]; float* colors=((float*)valPtr[3]); float constAttenuation=((float*)valPtr[4])[0]; float linAttenuation=((float*)valPtr[5])[0]; float quadAttenuation=((float*)valPtr[6])[0]; C7Vector lightTranformation(C4Vector((float*)valPtr[8]),C3Vector((float*)valPtr[7])); float lightSize=((float*)valPtr[9])[0]; float FadeXDistance=((float*)valPtr[10])[0]; // Pov-ray bool lightIsVisible=((bool*)valPtr[11])[0]; bool noShadow=((bool*)valPtr[12])[0]; // Pov-ray if (_simulationRunning||(!windowed)) { // Now set-up that light in OpenGl: C4X4Matrix m(lightTranformation.getMatrix()); GLfloat lightPos[]={0.0f,0.0f,0.0f,1.0f}; GLfloat lightDir[3]; if (lightType==sim_light_directional_subtype) { lightPos[0]=-m.M.axis[2](0); lightPos[1]=-m.M.axis[2](1); lightPos[2]=-m.M.axis[2](2); lightPos[3]=0.0f; } else { lightPos[0]=m.X(0); lightPos[1]=m.X(1); lightPos[2]=m.X(2); lightPos[3]=1.0f; } lightDir[0]=m.M.axis[2](0); lightDir[1]=m.M.axis[2](1); lightDir[2]=m.M.axis[2](2); glLightfv(GL_LIGHT0+activeLightCounter,GL_POSITION,lightPos); glLightfv(GL_LIGHT0+activeLightCounter,GL_SPOT_DIRECTION,lightDir); if (lightType==sim_light_omnidirectional_subtype) glLightf(GL_LIGHT0+activeLightCounter,GL_SPOT_CUTOFF,180.0f); if (lightType==sim_light_directional_subtype) glLightf(GL_LIGHT0+activeLightCounter,GL_SPOT_CUTOFF,90.0f); if (lightType==sim_light_spot_subtype) { float coa=cutoffAngle*radToDeg; if (coa>89.0f) // 90.0f causes problems on MacOS!!! coa=89.0f; glLightf(GL_LIGHT0+activeLightCounter,GL_SPOT_CUTOFF,coa); } glLightf(GL_LIGHT0+activeLightCounter,GL_SPOT_EXPONENT,float(spotExponent)); // glLighti & GL_SPOT_EXPONENT causes problems on MacOS!!! float black[4]={0.0f,0.0f,0.0f,1.0f}; glLightfv(GL_LIGHT0+activeLightCounter,GL_AMBIENT,black); float diffuseLight[4]={colors[3],colors[4],colors[5],1.0f}; glLightfv(GL_LIGHT0+activeLightCounter,GL_DIFFUSE,diffuseLight); float specularLight[4]={colors[6],colors[7],colors[8],1.0f}; glLightfv(GL_LIGHT0+activeLightCounter,GL_SPECULAR,specularLight); glLightf(GL_LIGHT0+activeLightCounter,GL_CONSTANT_ATTENUATION,constAttenuation); glLightf(GL_LIGHT0+activeLightCounter,GL_LINEAR_ATTENUATION,linAttenuation); glLightf(GL_LIGHT0+activeLightCounter,GL_QUADRATIC_ATTENUATION,quadAttenuation); glEnable(GL_LIGHT0+activeLightCounter); activeLightCounter++; } } if (message==sim_message_eventcallback_extrenderer_mesh) { // Collect mesh data from V-REP: void** valPtr=(void**)data; float* vertices=((float*)valPtr[0]); int verticesCnt=((int*)valPtr[1])[0]; int* indices=((int*)valPtr[2]); int triangleCnt=((int*)valPtr[3])[0]; float* normals=((float*)valPtr[4]); int normalsCnt=((int*)valPtr[5])[0]; float* colors=((float*)valPtr[8]); C7Vector tr(C4Vector((float*)valPtr[7]),C3Vector((float*)valPtr[6])); bool textured=((bool*)valPtr[18])[0]; float shadingAngle=((float*)valPtr[19])[0]; unsigned int meshId=((unsigned int*)valPtr[20])[0]; bool translucid=((bool*)valPtr[21])[0]; float opacityFactor=((float*)valPtr[22])[0]; bool backfaceCulling=((bool*)valPtr[23])[0]; int geomId=((int*)valPtr[24])[0]; int texId=((int*)valPtr[25])[0]; unsigned char* edges=((unsigned char*)valPtr[26]); bool visibleEdges=((bool*)valPtr[27])[0]; // valPtr[28] is reserved int povPatternType=((int*)valPtr[29])[0]; // pov-ray int displayAttrib=((int*)valPtr[30])[0]; const char* colorName=((char*)valPtr[31]); if (_simulationRunning||(!windowed)) { float* texCoords=NULL; int texCoordCnt=0; bool repeatU=false; bool repeatV=false; bool interpolateColors=false; int applyMode=0; COcTexture* theTexture=NULL; if (textured) { // Read some additional data from V-REP (i.e. texture data): texCoords=((float*)valPtr[9]); texCoordCnt=((int*)valPtr[10])[0]; unsigned char* textureBuff=((unsigned char*)valPtr[11]); // RGBA int textureSizeX=((int*)valPtr[12])[0]; int textureSizeY=((int*)valPtr[13])[0]; repeatU=((bool*)valPtr[14])[0]; repeatV=((bool*)valPtr[15])[0]; interpolateColors=((bool*)valPtr[16])[0]; applyMode=((int*)valPtr[17])[0]; theTexture=textureContainer->getFromId(texId); if (theTexture==NULL) { theTexture=new COcTexture(texId,textureBuff,textureSizeX,textureSizeY); textureContainer->add(theTexture); } } COcMesh* mesh=meshContainer->getFromId(geomId); if (mesh==NULL) { mesh=new COcMesh(geomId,vertices,verticesCnt*3,indices,triangleCnt*3,normals,normalsCnt*3,texCoords,texCoordCnt*2, edges); meshContainer->add(mesh); } mesh->render(tr,colors,textured,shadingAngle,translucid,opacityFactor,backfaceCulling,repeatU,repeatV,interpolateColors,applyMode,theTexture,visibleEdges); } } if (message==sim_message_eventcallback_extrenderer_triangles) { // Collect mesh data from V-REP: void** valPtr=(void**)data; float* vertices=((float*)valPtr[0]); int verticesCnt=((int*)valPtr[1])[0]; float* normals=((float*)valPtr[2]); float* colors=((float*)valPtr[3]); bool translucid=((bool*)valPtr[4])[0]; float opacityFactor=((float*)valPtr[5])[0]; int povPatternType=((int*)valPtr[6])[0]; // pov-ray if (_simulationRunning||(!windowed)) { // Now display the mesh with above data: glMateriali(GL_FRONT_AND_BACK,GL_SHININESS,48); float ambientDiffuse[4]={colors[0],colors[1],colors[2],opacityFactor}; glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,ambientDiffuse); float specular[4]={colors[6],colors[7],colors[8],1.0f}; glMaterialfv(GL_FRONT_AND_BACK,GL_SPECULAR,specular); float emission[4]={colors[9],colors[10],colors[11],1.0f}; glMaterialfv(GL_FRONT_AND_BACK,GL_EMISSION,emission); if (translucid) { glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); } glBegin(GL_TRIANGLES); for (int i=0;i<verticesCnt/3;i++) { glNormal3fv(normals+3*i); glVertex3fv(vertices+9*i+0); glVertex3fv(vertices+9*i+3); glVertex3fv(vertices+9*i+6); } glEnd(); glDisable(GL_BLEND); } } if (message==sim_message_eventcallback_extrenderer_stop) { void** valPtr=(void**)data; unsigned char* rgbBuffer=((unsigned char*)valPtr[0]); float* depthBuffer=((float*)valPtr[1]); bool readRgb=((bool*)valPtr[2])[0]; bool readDepth=((bool*)valPtr[3])[0]; if (windowed) { if (_simulationRunning) { COpenglWidget* oglWidget=getWidget(visionSensorOrCameraId); if (oglWidget!=NULL) { oglWidget->swapBuffers(); oglWidget->doneCurrentContext(); } } } else { COpenglOffscreen* oglOffscreen=getOffscreen(visionSensorOrCameraId); if (oglOffscreen!=NULL) { if (readRgb) { glPixelStorei(GL_PACK_ALIGNMENT,1); glReadPixels(0,0,resolutionX,resolutionY,GL_RGB,GL_UNSIGNED_BYTE,rgbBuffer); glPixelStorei(GL_PACK_ALIGNMENT,4); } if (readDepth) { glReadPixels(0,0,resolutionX,resolutionY,GL_DEPTH_COMPONENT,GL_FLOAT,depthBuffer); // Convert this depth info into values corresponding to linear depths (if perspective mode): if (perspectiveOperation) { float farMinusNear= farClippingPlane-nearClippingPlane; float farDivFarMinusNear=farClippingPlane/farMinusNear; float nearTimesFar=nearClippingPlane*farClippingPlane; int v=resolutionX*resolutionY; for (int i=0;i<v;i++) depthBuffer[i]=((nearTimesFar/(farMinusNear*(farDivFarMinusNear-depthBuffer[i])))-nearClippingPlane)/farMinusNear; } } oglOffscreen->doneCurrentContext(); } } if (_simulationRunning||(!windowed)) { meshContainer->decrementAllUsedCount(); meshContainer->removeAllUnused(); textureContainer->decrementAllUsedCount(); textureContainer->removeAllUnused(); } } }
{ X+=(Q*v.X); Q*=v.Q; } C3Vector C7Vector::operator* (const C3Vector& v) const { // Vector transformation return(X+(Q*v)); } void C7Vector::inverse() { (*this)=getInverse(); } C7Vector C7Vector::getInverse() const { C7Vector retV; retV.Q=Q.getInverse(); retV.X=(retV.Q*X)*-1.0f; return(retV); } void C7Vector::buildInterpolation(const C7Vector& fromThis,const C7Vector& toThat,float t) { // Builds the interpolation (based on t) from 'fromThis' to 'toThat' Q.buildInterpolation(fromThis.Q,toThat.Q,t); X.buildInterpolation(fromThis.X,toThat.X,t); } const C7Vector C7Vector::identityTransformation(C4Vector(1.0f,0.0f,0.0f,0.0f),C3Vector(0.0f,0.0f,0.0f));
void robot::createSensors() { std::string txt("There are "+boost::lexical_cast<std::string>(vSensors.size())+" sensors."); printToConsole(txt.c_str()); for(size_t i = 0; i < vSensors.size() ; i++) { sensor *Sensor = vSensors.at(i); if (Sensor->gazeboSpec) printToConsole("ERROR: sensor will not be created: the URDF specification is supported, but this is a Gazebo tag which is not documented as it seems."); else { if (Sensor->cameraSensorPresent) { int intParams[4]={Sensor->resolution[0],Sensor->resolution[1],0,0}; float floatParams[11]={Sensor->clippingPlanes[0],Sensor->clippingPlanes[1],60.0f*piVal/180.0f,0.2f,0.2f,0.4f,0.0f,0.0f,0.0f,0.0f,0.0f}; Sensor->nSensor=simCreateVisionSensor(1,intParams,floatParams,NULL); //Set the name: std::string baseName(Sensor->name+"_camera"); std::string fullName(baseName); int nsuff=2; while (simSetObjectName(Sensor->nSensor,fullName.c_str())==-1) fullName=baseName+boost::lexical_cast<std::string>(nsuff++); } int proxSensHandle=-1; if (Sensor->proximitySensorPresent) { // Proximity sensors seem to be very very specific and not general / generic at all. How come?! I.e. a succession of ray description (with min/max distances) would do int intParams[8]={16,16,1,4,16,1,0,0}; float floatParams[15]={0.0f,0.48f,0.1f,0.1f,0.1f,0.1f,0.0f,0.02f,0.02f,30.0f*piVal/180.0f,piVal/2.0f,0.0f,0.02f,0.0f,0.0f}; proxSensHandle=simCreateProximitySensor(sim_proximitysensor_cone_subtype,sim_objectspecialproperty_detectable_all,0,intParams,floatParams,NULL); //Set the name: std::string baseName(Sensor->name+"_proximity"); std::string fullName(baseName); int nsuff=2; while (simSetObjectName(proxSensHandle,fullName.c_str())==-1) fullName=baseName+boost::lexical_cast<std::string>(nsuff++); } // the doc doesn't state if a vision and proximity sensor can be declared at the same time... if (proxSensHandle!=-1) { if (Sensor->nSensor!=-1) { Sensor->nSensorAux=proxSensHandle; simSetObjectParent(Sensor->nSensorAux,Sensor->nSensor,true); } else Sensor->nSensor=proxSensHandle; } // Find the local configuration: C7Vector sensorLocal; sensorLocal.X.set(Sensor->origin_xyz); sensorLocal.Q.setEulerAngles(C3Vector(Sensor->origin_rpy)); C4Vector rot(0.0f,0.0f,piVal); // the V-REP sensors are rotated by 180deg around the Z-axis sensorLocal.Q=sensorLocal.Q*rot; // We attach the sensor to a link: C7Vector x; x.setIdentity(); int parentLinkIndex=getLinkPosition(Sensor->parentLink); if (parentLinkIndex!=-1) { int parentJointLinkIndex=getJointPosition(vLinks.at(parentLinkIndex)->parent); if (parentJointLinkIndex!=-1) x=vJoints.at(parentJointLinkIndex)->jointBaseFrame; } C7Vector sensorGlobal(x*sensorLocal); if (Sensor->nSensor!=-1) { simSetObjectPosition(Sensor->nSensor,-1,sensorGlobal.X.data); simSetObjectOrientation(Sensor->nSensor,-1,sensorGlobal.Q.getEulerAngles().data); } if ((parentLinkIndex!=-1)&&(Sensor->nSensor!=-1)) { if (vLinks.at(parentLinkIndex)->nLinkVisual!=-1) simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkVisual,true); if (vLinks.at(parentLinkIndex)->nLinkCollision!=-1) simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkCollision,true); } } } }
void robot::createLinks(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg) { std::string txt("There are "+boost::lexical_cast<std::string>(vLinks.size())+" links."); printToConsole(txt.c_str()); for(size_t i = 0; i < vLinks.size() ; i++) { link *Link = vLinks.at(i); Link->createLink(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg); // We attach the collision link to a joint. If the collision link isn't there, we use the visual link instead: C7Vector linkInitialConf; C7Vector linkDesiredConf; int effectiveLinkHandle=-1; if (Link->nLinkCollision!=-1) { effectiveLinkHandle=Link->nLinkCollision; linkDesiredConf.X.set(Link->collision_xyz); linkDesiredConf.Q.setEulerAngles(C3Vector(Link->collision_rpy)); } else { effectiveLinkHandle=Link->nLinkVisual; linkDesiredConf.X.set(Link->visual_xyz); linkDesiredConf.Q.setEulerAngles(C3Vector(Link->visual_rpy)); } simGetObjectPosition(effectiveLinkHandle,-1,linkInitialConf.X.data); C3Vector euler; simGetObjectOrientation(effectiveLinkHandle,-1,euler.data); linkInitialConf.Q.setEulerAngles(euler); C7Vector trAbs(linkDesiredConf*linkInitialConf); // still local int parentJointIndex=getJointPosition(Link->parent); if( parentJointIndex!= -1) { joint* Joint = vJoints.at(parentJointIndex); trAbs=Joint->jointBaseFrame*trAbs; // now absolute } //set the transfrom matrix to the object simSetObjectPosition(effectiveLinkHandle,-1,trAbs.X.data); simSetObjectOrientation(effectiveLinkHandle,-1,trAbs.Q.getEulerAngles().data); } // Finally the real parenting: for(size_t i = 0; i < vJoints.size() ; i++) { int parentLinkIndex=getLinkPosition(vJoints.at(i)->parentLink); if (parentLinkIndex!=-1) { link* parentLink=vLinks[parentLinkIndex]; if (parentLink->nLinkCollision!=-1) simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkCollision,true); else simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkVisual,true); } int childLinkIndex=getLinkPosition(vJoints.at(i)->childLink); if (childLinkIndex!=-1) { link* childLink=vLinks[childLinkIndex]; if (childLink->nLinkCollision!=-1) simSetObjectParent(childLink->nLinkCollision,vJoints.at(i)->nJoint,true); else simSetObjectParent(childLink->nLinkVisual,vJoints.at(i)->nJoint,true); } } }
void robot::createJoints(bool hideJoints,bool positionCtrl) { std::string txt("There are "+boost::lexical_cast<std::string>(vJoints.size())+" joints."); printToConsole(txt.c_str()); //Set parents and childs for all the links for(size_t i = 0; i < vJoints.size() ; i++) { vLinks.at(getLinkPosition(vJoints.at(i)->parentLink))->child = vJoints.at(i)->name; vLinks.at(getLinkPosition(vJoints.at(i)->childLink))->parent = vJoints.at(i)->name; } //Create the joints for(size_t i = 0; i < vJoints.size() ; i++) { //Move the joints to the positions specifieds by the urdf file C7Vector tmp; tmp.setIdentity(); tmp.X.set(vJoints.at(i)->origin_xyz); tmp.Q.setEulerAngles(C3Vector(vJoints.at(i)->origin_rpy)); vJoints.at(i)->jointBaseFrame=vJoints.at(i)->jointBaseFrame*tmp; //Set name jointParent to each joint int nParentLink = getLinkPosition(vJoints.at(i)->parentLink); vJoints.at(i)->parentJoint = vLinks.at(nParentLink)->parent; //Create the joint/forceSensor/dummy: if (vJoints.at(i)->jointType==-1) vJoints.at(i)->nJoint = simCreateDummy(0.02f,NULL); // when joint type was not recognized if (vJoints.at(i)->jointType==0) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==1) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_prismatic_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==2) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_spherical_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==3) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==4) { // when joint type is "fixed" int intParams[5]={1,4,4,0,0}; float floatParams[5]={0.02f,1.0f,1.0f,0.0f,0.0f}; vJoints.at(i)->nJoint = simCreateForceSensor(0,intParams,floatParams,NULL); } if ( (vJoints.at(i)->jointType==0)||(vJoints.at(i)->jointType==1) ) { float interval[2]={vJoints.at(i)->lowerLimit,vJoints.at(i)->upperLimit-vJoints.at(i)->lowerLimit}; simSetJointInterval(vJoints.at(i)->nJoint,0,interval); if (vJoints.at(i)->jointType==0) { // revolute simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitAngular); simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitAngular); } else { // prismatic simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitLinear); simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitLinear); } // We turn the position control on: if (positionCtrl) { simSetObjectIntParameter(vJoints.at(i)->nJoint,2000,1); simSetObjectIntParameter(vJoints.at(i)->nJoint,2001,1); } } //Set the name: std::string baseName(vJoints.at(i)->name); std::string fullName(baseName); int nsuff=2; while (simSetObjectName(vJoints.at(i)->nJoint,fullName.c_str())==-1) fullName=baseName+boost::lexical_cast<std::string>(nsuff++); if (hideJoints) simSetObjectIntParameter(vJoints.at(i)->nJoint,10,512); // layer 10 } //Set positions to joints from the 4x4matrix for(size_t i = 0; i < vJoints.size() ; i++) { simSetObjectPosition(vJoints.at(i)->nJoint,-1,vJoints.at(i)->jointBaseFrame.X.data); simSetObjectOrientation(vJoints.at(i)->nJoint,-1 ,vJoints.at(i)->jointBaseFrame.Q.getEulerAngles().data); } //Set joint parentship between them (thes parentship will be remove before adding the joints) for(size_t i = 0; i < vJoints.size() ; i++) { int parentJointIndex=getJointPosition(vJoints.at(i)->parentJoint); if ( parentJointIndex!= -1) { simInt nParentJoint = vJoints.at(parentJointIndex)->nJoint; simInt nJoint = vJoints.at(i)->nJoint; simSetObjectParent(nJoint,nParentJoint,false); } } //Delete all the partnership without moving the joints but after doing that update the transform matrix for(size_t i = 0; i < vJoints.size() ; i++) { C4X4Matrix tmp; simGetObjectPosition(vJoints.at(i)->nJoint,-1,tmp.X.data); C3Vector euler; simGetObjectOrientation(vJoints.at(i)->nJoint,-1,euler.data); tmp.M.setEulerAngles(euler); vJoints.at(i)->jointBaseFrame = tmp; simInt nJoint = vJoints.at(i)->nJoint; simSetObjectParent(nJoint,-1,true); } for(size_t i = 0; i < vJoints.size() ; i++) { C4X4Matrix jointAxisMatrix; jointAxisMatrix.setIdentity(); C3Vector axis(vJoints.at(i)->axis); C3Vector rotAxis; float rotAngle=0.0f; if (axis(2)<1.0f) { if (axis(2)<=-1.0f) rotAngle=3.14159265359f; else rotAngle=acosf(axis(2)); rotAxis(0)=-axis(1); rotAxis(1)=axis(0); rotAxis(2)=0.0f; rotAxis.normalize(); C7Vector m(jointAxisMatrix); float alpha=-atan2(rotAxis(1),rotAxis(0)); float beta=atan2(-sqrt(rotAxis(0)*rotAxis(0)+rotAxis(1)*rotAxis(1)),rotAxis(2)); C7Vector r; r.X.clear(); r.Q.setEulerAngles(0.0f,0.0f,alpha); m=r*m; r.Q.setEulerAngles(0.0f,beta,0.0f); m=r*m; r.Q.setEulerAngles(0.0f,0.0f,rotAngle); m=r*m; r.Q.setEulerAngles(0.0f,-beta,0.0f); m=r*m; r.Q.setEulerAngles(0.0f,0.0f,-alpha); m=r*m; jointAxisMatrix=m.getMatrix(); } C4Vector q((vJoints.at(i)->jointBaseFrame*jointAxisMatrix).Q); simSetObjectOrientation(vJoints.at(i)->nJoint,-1,q.getEulerAngles().data); } }
bool CGeometricConstraintSolver::solve(CIKGraphObjCont& graphContainer,SGeomConstrSolverParam& parameters) { if (graphContainer.identifyElements()==0) return(false); // Nothing to solve (no active joint in the mechanism) graphContainer.putElementsInPlace(); // We create a branched tree, where each extremity has a tip dummy // (which will later be constrained to its respective target dummy) CIKGraphObject* baseIKGraphObject=graphContainer.getBaseObject(); CIKGraphNode* graphIterator=baseIKGraphObject; CIKGraphNode* previousPosition=NULL; CIKGraphNode* nextPosition=NULL; C7Vector localTransformation; localTransformation.setIdentity(); CIKObjCont ikObjs; CIKJoint* lastJoint=NULL; CIKJoint* treeHandle=NULL; // Some precalculations of some fixed rotations: C4X4Matrix tmpRot; tmpRot.setIdentity(); tmpRot.M(0,0)=-1.0f; tmpRot.M(2,2)=-1.0f; C7Vector rotY180(tmpRot.getTransformation()); tmpRot.M.clear(); tmpRot.M(0,0)=1.0f; tmpRot.M(2,1)=1.0f; tmpRot.M(1,2)=-1.0f; C7Vector rotX90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f)); tmpRot.M.clear(); tmpRot.M(2,0)=-1.0f; tmpRot.M(1,1)=1.0f; tmpRot.M(0,2)=1.0f; C7Vector rotY90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f)); tmpRot.M.clear(); tmpRot.M(1,0)=1.0f; tmpRot.M(0,1)=-1.0f; tmpRot.M(2,2)=1.0f; C7Vector rotZ90(tmpRot.getTransformation().Q,C3Vector(0.0f,0.0f,0.0f)); std::vector<CIKGraphNode*> graphObjectsToBeExplored; graphObjectsToBeExplored.push_back(baseIKGraphObject); std::vector<CIKJoint*> lastJoints; lastJoints.push_back(NULL); std::vector<CIKGraphNode*> previousPositions; previousPositions.push_back(NULL); std::vector<C7Vector> localTransformations; localTransformations.push_back(localTransformation); int explorationID=0; while (graphObjectsToBeExplored.size()!=0) { graphIterator=graphObjectsToBeExplored.back(); graphObjectsToBeExplored.pop_back(); lastJoint=lastJoints.back(); lastJoints.pop_back(); previousPosition=previousPositions.back(); previousPositions.pop_back(); localTransformation=localTransformations.back(); localTransformations.pop_back(); bool doIt=(graphIterator->explorationID==-1); bool goingDown=false; bool closeComplexLoop=false; while (doIt) { if (graphIterator->explorationID==-1) graphIterator->explorationID=explorationID; explorationID++; C7Vector previousCT; if (previousPosition!=NULL) { if (previousPosition->type==IK_GRAPH_JOINT_TYPE) previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation; else previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation; } else { previousCT=baseIKGraphObject->cumulativeTransformation; localTransformation=previousCT; } if (graphIterator->type==IK_GRAPH_JOINT_TYPE) { // Joint: we have to introduce a joint CIKGraphJoint* graphJoint=(CIKGraphJoint*)graphIterator; if (!graphJoint->disabled) { C7Vector sphTr; sphTr.setIdentity(); sphTr.Q=graphJoint->sphericalTransformation; CIKJoint* newIKJoint; if (graphJoint->jointType==IK_GRAPH_SPHERICAL_JOINT_TYPE) { int dataValueBase=10*graphJoint->nodeID; CIKJoint* avatarParent; if (graphJoint->topObject==(CIKGraphObject*)previousPosition) { // From tip to base C7Vector rel(localTransformation*rotY180); newIKJoint=new CIKJoint(graphJoint,rel,false,false); if (lastJoint==NULL) { treeHandle=newIKJoint; lastJoint=treeHandle; ikObjs.addRoot(lastJoint); } else { ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; } avatarParent=ikObjs.getJointWithData(dataValueBase+3); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+3; rel=rotX90; newIKJoint=new CIKJoint(graphJoint,rel,false,false); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+2); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+2; rel=rotY90; newIKJoint=new CIKJoint(graphJoint,rel,false,false); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+1); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+1; rel=rotX90*rotZ90.getInverse()*sphTr.getInverse()*rotY180; newIKJoint=new CIKJoint(graphJoint,rel,true,false); lastJoint->topJoint=newIKJoint; // This is mainly needed by the joint-limitation part! ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; lastJoint->active=false; // Inactive for now (we can activate it later) avatarParent=ikObjs.getJointWithData(dataValueBase+0); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+0; localTransformation=rotY180; } else { // From base to tip C7Vector rel(localTransformation); newIKJoint=new CIKJoint(graphJoint,rel,false,true); if (lastJoint==NULL) { treeHandle=newIKJoint; lastJoint=treeHandle; ikObjs.addRoot(lastJoint); } else { ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; } lastJoint->active=false; // Inactive for now (we can activate it later) avatarParent=ikObjs.getJointWithData(dataValueBase+0); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+0; rel=sphTr*rotY90; newIKJoint=new CIKJoint(graphJoint,rel,false,true); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+1); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+1; rel=rotX90.getInverse(); newIKJoint=new CIKJoint(graphJoint,rel,false,true); ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+2); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+2; rel=rotY90.getInverse()*rotZ90.getInverse(); newIKJoint=new CIKJoint(graphJoint,rel,true,true); newIKJoint->topJoint=newIKJoint; // Top-joint is itself! ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; avatarParent=ikObjs.getJointWithData(dataValueBase+3); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValueBase+3; localTransformation.setIdentity(); } } else { if (graphJoint->topObject==(CIKGraphObject*)previousPosition) { // From tip to base C7Vector rel(localTransformation*rotY180); newIKJoint=new CIKJoint(graphJoint,rel,false,false); localTransformation=rotY180; } else { // From base to tip C7Vector rel(localTransformation); newIKJoint=new CIKJoint(graphJoint,rel,false,false); localTransformation.setIdentity(); } if (lastJoint==NULL) { treeHandle=newIKJoint; lastJoint=treeHandle; ikObjs.addRoot(lastJoint); } else { ikObjs.addChild(lastJoint,newIKJoint); lastJoint=newIKJoint; } int dataValue=10*graphJoint->nodeID+0; CIKJoint* avatarParent=ikObjs.getJointWithData(dataValue); if (avatarParent!=NULL) // This joint is used twice (going up and going down) avatarParent->addAvatar(lastJoint); lastJoint->data=dataValue; } } else { // In case a graph-joint is disabled: if (graphJoint->topObject==(CIKGraphObject*)previousPosition) { // From tip to base localTransformation=localTransformation*graphJoint->getDownToTopTransformation().getInverse(); } else { // From base to tip localTransformation=localTransformation*graphJoint->getDownToTopTransformation(); } } } else { CIKGraphObject* theObject=(CIKGraphObject*)graphIterator; if (theObject->objectType==IK_GRAPH_LINK_OBJECT_TYPE) { // Link if (previousPosition!=NULL) { if (theObject->linkPartner!=previousPosition) localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation; // If (theObject->linkPartner==previousPosition) then we don't do anything! } } else { // Here we have a dummy we have to assign to a configuration or a passive object // We treat all cases first as passive objects: if (previousPosition!=NULL) { localTransformation=localTransformation*previousCT.getInverse()*theObject->cumulativeTransformation; if ( (theObject->objectType==IK_GRAPH_TIP_OBJECT_TYPE)&&(lastJoint!=NULL) ) { // This is a valid dummy-tip! CIKDummy* newIKDummy=new CIKDummy(localTransformation,theObject->targetCumulativeTransformation); ikObjs.addChild(lastJoint,newIKDummy); newIKDummy->constraints=(IK_X_CONSTRAINT|IK_Y_CONSTRAINT|IK_Z_CONSTRAINT); newIKDummy->dampingFactor=1.0f; newIKDummy->loopClosureDummy=false; if (graphIterator->getConnectionNumber()==1) break; } } } } int unexploredSize=graphIterator->getNumberOfUnexplored(); if ( (unexploredSize==0)||goingDown||closeComplexLoop ) { if ( (graphIterator->getConnectionNumber()==1)&&(!closeComplexLoop) ) break; // This is a rare case where we have an endpoint without a tip-dummy mobile-part if (closeComplexLoop) { CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation); ikObjs.addChild(lastJoint,tipDummy); break; } nextPosition=graphIterator->getExploredWithSmallestExplorationID(); if ( (nextPosition->explorationID==0)&&(!goingDown) ) { // The loop can now be closed (simple loop with each joint present at most once) previousCT=((CIKGraphObject*)graphIterator)->cumulativeTransformation; localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation; CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation); ikObjs.addChild(lastJoint,tipDummy); break; } if ( (nextPosition->explorationID==0)&&goingDown ) closeComplexLoop=true; goingDown=true; } else if ((graphIterator->getNeighbourWithExplorationID(0)!=NULL)&&(!goingDown)&&(previousPosition->explorationID!=0)) { // Here we have to close the loop too! // We first put unexplored paths onto the stack: for (int i=0;i<unexploredSize;i++) { // We throw unexplored nodes onto the exploration stack: graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i)); lastJoints.push_back(lastJoint); previousPositions.push_back(graphIterator); localTransformations.push_back(localTransformation); } nextPosition=graphIterator->getExploredWithSmallestExplorationID(); previousCT=((CIKGraphObject*)previousPosition)->cumulativeTransformation; localTransformation=localTransformation*previousCT.getInverse()*((CIKGraphObject*)nextPosition)->cumulativeTransformation; CIKDummy* tipDummy=new CIKDummy(localTransformation,baseIKGraphObject->cumulativeTransformation); ikObjs.addChild(lastJoint,tipDummy); break; } else { if (previousPosition==NULL) { // This is the start. We should always explore first two links which belong together // or the 3 objects making up a joint! nextPosition=NULL; for (int i=0;i<unexploredSize;i++) { CIKGraphNode* nextPositionTmp=graphIterator->getUnexplored(i); if ( (((CIKGraphObject*)graphIterator)->linkPartner==nextPositionTmp)|| (nextPositionTmp->type==IK_GRAPH_JOINT_TYPE) ) nextPosition=nextPositionTmp; else { graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i)); lastJoints.push_back(lastJoint); previousPositions.push_back(graphIterator); localTransformations.push_back(localTransformation); if (nextPosition==NULL) nextPosition=graphIterator->getUnexplored(i); } } } else { nextPosition=graphIterator->getUnexplored(0); for (int i=1;i<unexploredSize;i++) { // We throw unexplored nodes onto the exploration stack: graphObjectsToBeExplored.push_back(graphIterator->getUnexplored(i)); lastJoints.push_back(lastJoint); previousPositions.push_back(graphIterator); localTransformations.push_back(localTransformation); } } } previousPosition=graphIterator; graphIterator=nextPosition; } } solveHierarchy(&ikObjs,parameters); for (int i=0;i<int(ikObjs.allObjects.size());i++) { CIKObject* it=ikObjs.allObjects[i]; if (it->objectType==IK_JOINT_TYPE) { CIKJoint* theJoint=(CIKJoint*)it; if (theJoint->avatarParent==NULL) { if (theJoint->spherical) { if (theJoint->topSpherical) { float a0=theJoint->parameter; float a1=((CIKJoint*)theJoint->parent)->parameter; float a2=((CIKJoint*)theJoint->parent->parent)->parameter; float a3=((CIKJoint*)theJoint->parent->parent->parent)->parameter; if (theJoint->sphericalUp) { theJoint->graphJoint->sphericalTransformation=C4Vector(a3,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a2,a1,a0)); } else { theJoint->graphJoint->sphericalTransformation=C4Vector(a0,C3Vector(0.0f,0.0f,1.0f))*theJoint->graphJoint->sphericalTransformation*C4Vector(C3Vector(a1,a2,a3)); } } } else theJoint->graphJoint->parameter=theJoint->parameter; } } } graphContainer.actualizeAllTransformations(); graphContainer.putElementsInPlace(); return(true); }