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