Пример #1
0
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

}
Пример #2
0
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

}