void drawRedDotsPlane()
{ // Draw the stimulus ( red-dots plane )
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
// IMPORTANT Reset the previous status of transformation
objectActiveTransformation.setIdentity();
objectActiveTransformation.translation() = projPointEyeRight + translationFactor;
if ((int)factors["Translation"]==-1 || (int)factors["Translation"]==-2 )
objectActiveTransformation.linear().setIdentity();
else
objectActiveTransformation.linear() = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
glPushMatrix(); // PUSH MATRIX
glLoadIdentity();
glMultMatrixd(objectActiveTransformation.data());
Vector3d posAlongLineOfSight = (headEyeCoords.getRigidStart().getFullTransformation().rotation())*(eyeRight-eyeCalibration);
double argslant = acos( cos(toRadians(factors["Slant"]))*(focalDistance-posAlongLineOfSight.z() )/((focalDistance )));
instantPlaneSlant = toDegrees(argslant);
switch ( (int) factors["Tilt"] )
{
case 0:
glRotated( instantPlaneSlant ,0,1,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
glScaled(1/sin(toRadians( -90-factors["Slant"])),1,1); //backprojection phase
break;
case 90:
glRotated( -instantPlaneSlant ,1,0,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
break;
case 180:
glRotated( -instantPlaneSlant ,0,1,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
glScaled(1/sin(toRadians( -90-factors["Slant"] )),1,1); //backprojection phase
break;
case 270:
glRotated( instantPlaneSlant ,1,0,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
break;
}
glGetDoublev(GL_MODELVIEW_MATRIX,objectActiveTransformation.data());
BoundChecker stimBoundariesActive(&cam, &redDotsPlane);
BoundChecker stimBoundariesPassive(&camPassive, &redDotsPlane);
stimOutside = ( stimBoundariesActive.checkOutside(objectActiveTransformation) || stimBoundariesPassive.checkOutside(objectActiveTransformation));
stimDrawer.draw();
glPopMatrix(); // POP MATRIX
}
Affine3d FromODETransformToEigenTransform(const dReal* pos, const dReal* rot)
{
Affine3d ret;
Vector3d trans;
trans << pos[0], pos[1], pos[2];
ret.translation() = trans;
Matrix3d rotation = FromODERotationToEigenMatrix(rot);
ret.linear() = rotation;
return ret;
}
geometry_msgs::Pose MoveitPlanningInterface::transformEigenAffine3dToPose(Affine3d e) {
Vector3d Oe;
Matrix3d Re;
geometry_msgs::Pose pose;
Oe = e.translation();
Re = e.linear();
Quaterniond q(Re); // convert rotation matrix Re to a quaternion, q
pose.position.x = Oe(0);
pose.position.y = Oe(1);
pose.position.z = Oe(2);
pose.orientation.x = q.x();
pose.orientation.y = q.y();
pose.orientation.z = q.z();
pose.orientation.w = q.w();
return pose;
}
void drawFixation()
{
switch ( headCalibrationDone )
{
case 1:
// Fixed stimulus
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glEnd();
glPointSize(1);
break;
case 2:
// Fixed stimulus + projected points
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glColor3fv(glRed);
glVertex3dv(projPointEyeRight.data());
glColor3fv(glBlue);
glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
glEnd();
glPointSize(1);
// Draw the calibration circle
glColor3fv(glWhite);
break;
case 3:
{
// DRAW THE FIXATION POINT
double eyeToCenterAngleX= toDegrees(atan(eyeRight.x()/(-focalDistance-eyeRight.z()) ));
double eyeToCenterAngleY= toDegrees(atan(eyeRight.y()/(-focalDistance-eyeRight.z()) ));
double projPointAngleX = toDegrees( atan( (projPointEyeRight.x()-eyeRight.x())/abs(projPointEyeRight.z())));
double maxAllowedTranslationYaw = str2num<double>(parameters.find("MaxAllowedTranslationYaw"));
Vector3d stimulusCenter(0,0,0);
Matrix3d objrotation = Matrix3d::Identity();
// IMPORTANT Reset the previous status of transformation
objectActiveTransformation.setIdentity();
switch ( (int) factors["Rotation"] )
{
case 2:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw()*factors["FollowingSpeed"], Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getYaw())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 1:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch()*factors["FollowingSpeed"], Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getPitch())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 0:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
//instantPlaneSlant = eyeRight.x()*factors["RotationSpeed"]/10+factors["Slant"];
instantPlaneSlant = toDegrees( atan(eyeRight.x()/abs(focalDistance+eyeRight.z()) ) )*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(eyeRight.x()*factors["FollowingSpeed"],eyeRight.y(),eyeRight.z()+focalDistance);
objectActiveTransformation.linear() = objrotation;
}
break;
}
objectActiveTransformation.translation() = stimulusCenter;
Vector3d fixationPointTmp = objectActiveTransformation.translation();
glPushMatrix();
glTranslated(fixationPointTmp.x(),fixationPointTmp.y(),fixationPointTmp.z());
glutSolidSphere(1,10,10);
glPopMatrix();
break;
}
}
}
void drawRedDotsPlane()
{ // Draw the stimulus ( red-dots plane )
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
Matrix3d objrotation ;
Vector3d stimulusCenter;
double instantPlaneSlant=0;
// IMPORTANT Reset the previous status of transformation
objectActiveTransformation.setIdentity();
switch ( (int) factors["Rotation"] )
{
case 2:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw()*factors["FollowingSpeed"], Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getYaw())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 1:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch()*factors["FollowingSpeed"], Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getPitch())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 0:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
//instantPlaneSlant = eyeRight.x()*factors["RotationSpeed"]/10+factors["Slant"];
instantPlaneSlant = toDegrees( atan(eyeRight.x()/abs(focalDistance+eyeRight.z()) ) )+factors["Slant"];
stimulusCenter = headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(eyeRight.x()*factors["FollowingSpeed"],eyeRight.y(),eyeRight.z()+focalDistance);
objectActiveTransformation.linear() = objrotation;
}
break;
}
objectActiveTransformation.translation() = stimulusCenter;
//cerr << instantPlaneSlant << endl;
glPushMatrix(); // PUSH MATRIX
glLoadIdentity();
glMultMatrixd(objectActiveTransformation.data());
switch ( (int) factors["Tilt"] )
{
case 0:
glRotated( instantPlaneSlant ,0,1,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
glScaled(1/sin(toRadians( -90-factors["Slant"])),1,1); //backprojection phase
break;
case 90:
glRotated( -instantPlaneSlant ,1,0,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
break;
case 180:
glRotated( -instantPlaneSlant ,0,1,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitY() );
glScaled(1/sin(toRadians( -90-factors["Slant"] )),1,1); //backprojection phase
break;
case 270:
glRotated( instantPlaneSlant ,1,0,0);
//objectActiveTransformation*=AngleAxisd( toRadians(-instantPlaneSlant), Vector3d::UnitX() );
glScaled(1,1/sin(toRadians( -90-factors["Slant"] )),1); //backprojection phase
break;
}
stimDrawer.draw();
glPopMatrix(); // POP MATRIX
}