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);
}
