Ejemplo n.º 1
0
vqs vqs::operator+( const vqs&	pVQS )
{
	CVector3 transAdd = translation() + pVQS.translation();
	DQuaternion quatAdd = orientation() + pVQS.orientation();
	float scaleAdd = scale() + pVQS.scale(); 
	vqs result;
	result.Set(transAdd, quatAdd, scaleAdd);
	return result;
}
Ejemplo n.º 2
0
// Position and Rotation
const LLMatrix4&  	LLMatrix4::initRotTrans(const F32 angle, const F32 rx, const F32 ry, const F32 rz,
											const F32 tx, const F32 ty, const F32 tz)
{
	LLMatrix3	mat(angle, rx, ry, rz);
	LLVector3	translation(tx, ty, tz);
	initMatrix(mat);
	setTranslation(translation);
	return (*this);
}
Ejemplo n.º 3
0
/*!
    Returns the transformation to apply to the modelview matrix
    to present the scene as viewed from the eye position.

    The \a eye parameter is used to adjust the camera's position
    horizontally by half of eyeSeparation() if \a eye is QGL::LeftEye
    or QGL::RightEye.

    \sa projectionMatrix()
*/
QMatrix4x4 QGLCamera::modelViewMatrix(QGL::Eye eye) const
{
    Q_D(const QGLCamera);
    QMatrix4x4 m;
    QVector3D adjust;
    if (eye == QGL::LeftEye)
        adjust = translation(-d->eyeSeparation / 2.0f, 0.0f, 0.0f);
    else if (eye == QGL::RightEye)
        adjust = translation(d->eyeSeparation / 2.0f, 0.0f, 0.0f);
    if (d->motionQuaternion.isIdentity()) {
        m.lookAt(d->eye + adjust, d->center, d->upVector);
    } else {
        QVector3D up = d->motionQuaternion.rotatedVector(d->upVector);
        QVector3D view = d->motionQuaternion.rotatedVector(d->viewVector);
        m.lookAt(d->center - view + adjust, d->center, up);
    }
    return m;
}
Ejemplo n.º 4
0
void RotateGizmoRing::render(IViewer* viewer) const {
  GraphicsInterface::beginPerformanceEvent("Ring");

  GraphicsInterface::resetRenderTarget(true);
  GraphicsInterface::setViewport(GraphicsInterface::backBufferSize());
  GraphicsInterface::setBlendState(IGraphicsInterface::NOBLEND);
  GraphicsInterface::setRenderState(CULL_MODE_NONE, true);

  if (drawBounds_)
  {
    effect_->beginDraw();

    BoundingBox bounds = boundingBox();

    Vector4 scale(bounds.max.x - bounds.min.x, bounds.max.y - bounds.min.y, bounds.max.z - bounds.min.z, 2.0f);
    scale /= 2.0f;

    Vector4 translation(bounds.max.x + bounds.min.x, bounds.max.y + bounds.min.y, bounds.max.z + bounds.min.z, 2.0f);
    translation /= 2.0f;

    Matrix4x4 model = localToWorld() * Matrix4x4::translation(translation) * Matrix4x4::scale(scale);
    Matrix4x4 modelViewProjection = viewer->projection() * viewer->viewTransform() * model * Matrix4x4::scale(1.0001f);

    effect_->setUniform(modelViewProjection, "ModelViewProj");
    effect_->setUniform(ringColor_, "Color");

    effect_->commitBuffers();
    GraphicsInterface::drawVertexBuffer(Geometry::UNIT_CUBE_VERTEX_BUFFER, Geometry::UNIT_CUBE_VERTEX_COUNT, Geometry::UNIT_CUBE_VERTEX_FORMAT);
    effect_->endDraw();
  }

  // ring
  { 
    effect_->beginDraw();

    Color3 color = ringColor_;

    if (isHighlighted_) {
      color = Color3::YELLOW;
    }

    effect_->setUniform(color, "Color");

    Matrix4x4 modelViewProjection = viewer->projection() * viewer->viewTransform() * localToWorld();
    effect_->setUniform(modelViewProjection, "ModelViewProj");
    
    Matrix4x4 model = localToWorld();
    effect_->setUniform(model, "Model");

    effect_->commitBuffers();
    GraphicsInterface::drawVertexBuffer(Geometry::UNIT_CIRCLE_VERTEX_BUFFER, Geometry::UNIT_CIRCLE_VERTEX_COUNT, Geometry::UNIT_CIRCLE_VERTEX_FORMAT);
    effect_->endDraw();
  }

  GraphicsInterface::endPerformanceEvent();
}
Ejemplo n.º 5
0
void GeneralSettingsPage::apply()
{
	auto language = language_box->currentData();
	if (language != getSetting(Settings::General_Language)
	    || translation_file != getSetting(Settings::General_TranslationFile).toString())
	{
		// Show an message box in the new language.
		TranslationUtil translation((QLocale::Language)language.toInt(), translation_file);
		auto new_language = translation.getLocale().language();
		switch (new_language)
		{
		case QLocale::AnyLanguage:
		case QLocale::C:
		case QLocale::English:
			QMessageBox::information(window(), QLatin1String("Notice"), QLatin1String("The program must be restarted for the language change to take effect!"));
			break;
			
		default:
			qApp->installEventFilter(this);
			qApp->installTranslator(&translation.getQtTranslator());
			qApp->installTranslator(&translation.getAppTranslator());
			QMessageBox::information(window(), tr("Notice"), tr("The program must be restarted for the language change to take effect!"));
			qApp->removeTranslator(&translation.getAppTranslator());
			qApp->removeTranslator(&translation.getQtTranslator());
			qApp->removeEventFilter(this);
		}
		
		setSetting(Settings::General_Language, new_language);
		setSetting(Settings::General_TranslationFile, translation_file);
#if defined(Q_OS_MAC)
		// The native [file] dialogs will use the first element of the
		// AppleLanguages array in the application's .plist file -
		// and this file is also the one used by QSettings.
		const QString mapper_language(translation.getLocale().name().left(2));
		QSettings().setValue(QLatin1String{"AppleLanguages"}, { mapper_language });
#endif
	}
	
	setSetting(Settings::General_OpenMRUFile, open_mru_check->isChecked());
	setSetting(Settings::HomeScreen_TipsVisible, tips_visible_check->isChecked());
	setSetting(Settings::General_NewOcd8Implementation, ocd_importer_check->isChecked());
	setSetting(Settings::General_RetainCompatiblity, compatibility_check->isChecked());
	setSetting(Settings::General_PixelsPerInch, ppi_edit->value());
	
	auto name_latin1 = encoding_box->currentText().toLatin1();
	if (name_latin1 == "System"
	    || QTextCodec::codecForName(name_latin1))
	{
		setSetting(Settings::General_Local8BitEncoding, name_latin1);
	}
	
	int interval = autosave_interval_edit->value();
	if (!autosave_check->isChecked())
		interval = -interval;
	setSetting(Settings::General_AutosaveInterval, interval);
}
Ejemplo n.º 6
0
void MapBuilder::onCloudFrame2(pcl::PointCloud<pcl::PointXYZ>::Ptr frame, pcl::PointXY sensorOffset, RobotPosition pose)
{


    //Transform current cloud to match robot's position
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);
    double mx =  pose.X + sensorOffset.x;
    double my =  pose.Y + sensorOffset.y;
    double mt = AngleUtils::degToRads(pose.Heading);
    Eigen::Vector3f translation(mx, my, 0);
    Eigen::Quaternionf rotation(Eigen::AngleAxisf(-mt, Eigen::Vector3f::UnitZ()));
    pcl::transformPointCloud(*frame, *cloud_transformed, translation, rotation);

    // ICP to refine cloud alignment
    if(false)//!cloud->empty())
    {
        //Use ICP to fix errors in transformation
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        icp.setMaxCorrespondenceDistance(2);
        icp.setMaximumIterations(30);
        icp.setInputSource(cloud_transformed);
        icp.setInputTarget(cloud);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned (new pcl::PointCloud<pcl::PointXYZ>);
        icp.align(*cloud_aligned);

        if(icp.hasConverged())
        {
            //Add aligned cloud points to map cloud
            (*cloud) += *cloud_aligned;
        }
        else
        {
            //Ignore alignment atempt
            std::cout << "No convergence." << std::endl;
            (*cloud) += *cloud_transformed;
        }
    }
    else
    {
        //Add first cloud's points to map cloud
        (*cloud) += *cloud_transformed;
    }

    //Run map cloud through a VoxelGrid to remove any duplicate points
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_removedDuplicates (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> duplicateRemover;

    duplicateRemover.setInputCloud(cloud);
    float leafSize = ConfigManager::Instance().getValue("MapBuilder", "VoxelLeafSize", 0.25);
    duplicateRemover.setLeafSize (leafSize, leafSize, leafSize);
    duplicateRemover.filter(*cloud_removedDuplicates);

    cloud.swap(cloud_removedDuplicates);

    onNewMap(cloud);
}
Ejemplo n.º 7
0
void IlinTest::test_translation()
{
    double radians = 0.5;
    Angle angle;
    translation(radians, &angle);
    QCOMPARE(angle.degree, 28);
    QCOMPARE(angle.min, 39);
    QCOMPARE(angle.sec, 44);

}
Ejemplo n.º 8
0
void Translation::run(vector<string> pArgParam,string input, string output){
	unused(pArgParam);
	ImagePNG* img = new ImagePNG();
	img->load(input);
	double start = Data::get_time();
	cout << &img << endl;
	translation(img,atoi(pArgParam[0].c_str()),atoi(pArgParam[1].c_str()), new Color(0,0,0,255));
	if (Data::CHRONO)	cout << description<<" effectue en " << 1000*(Data::get_time() - start)<<" ms"<<endl;
	img->save(output);
}
Ejemplo n.º 9
0
void bimWorld::MovingCamera::translation(double startx, double starty, double endx, double endy)
{
	auto manip = static_cast<PersonManipulator*>(m_host->_CameraManipulator()->getCameraManipulator());
	if (!manip)
		return;

	manip->translation(startx, starty, endx, endy, m_host->_CameraManipulator()->getCamera());

	m_host->_RenderingThreads()->updateSeveralTimes();
}
Ejemplo n.º 10
0
Matrix4f Transform::getTransform() {

	Matrix4f translation(this->translation, MATRIX4F_INIT_WITH_TRANSLATION);
	Matrix4f rotation(this->rotation, MATRIX4F_INIT_WITH_ROTATION);
	Matrix4f scale(this->scale, MATRIX4F_INIT_WITH_SCALE);
//	Matrix4f cameraRotation(this->camera.getForward(), this->camera.getUp());
//	Matrix4f cameraTranslation(-this->camera.getPos().getX(),-this->camera.getPos().getY(),-this->camera.getPos().getZ(),MATRIX4F_INIT_WITH_TRANSLATION);

	return translation * rotation * scale;
}
Ejemplo n.º 11
0
	Edge steerWithControl(const State &start, const std::vector<double> controls, double dt) const {
		/* Be careful about the order these are being passed in */
		fcl::Vec3f translation(controls[0], controls[1], controls[2]);
		fcl::Quaternion3f rotation(controls[3], controls[4], controls[5], controls[6]);
		rotation = math::normalize(rotation);

		State end = doSteps(start, translation, rotation, dt);

		return Edge(start, end, translation, rotation, dt);
	}
Ejemplo n.º 12
0
void CameraObject::look_at (const Vector3 &to, const Vector3 &up)
{
    Matrix3 m;	
    Vector3 delta;
    
    delta = translation() - to;
    
    m = Matrix3::set_orientation(delta, up);
    set_orientation(m);
}
Ejemplo n.º 13
0
Matrix4d Sim3
::matrix() const
{
  Matrix4d homogenious_matrix;
  homogenious_matrix.setIdentity();
  homogenious_matrix.block(0,0,3,3) = scale()*rotation_matrix();
  homogenious_matrix.col(3).head(3) = translation();
  homogenious_matrix(3,3) = 1;
  return homogenious_matrix;
}
Ejemplo n.º 14
0
void RenderLine2D(Shader *sh, Primitive prim, const float3 &v1, const float3 &v2, float thickness)
{
    auto v = (v2 - v1) / 2;
    auto len = length(v);
    auto vnorm = v / len;
    auto trans = translation(v1 + v) * 
                 rotationZ(vnorm.xy()) *
                 float4x4(float4(len, thickness / 2, 1, 1));
    RenderQuad(sh, prim, true, trans);
}
Ejemplo n.º 15
0
void NGLScene::writeKey()
{
  std::cout << "KEY!" << std::endl;
  ngl::Vec3 translation(0,0,0);
  for(unsigned int i=0; i<m_geo.size(); i++ ){
    m_rotHistory.push_back(m_geo[i]->m_rotation);
    m_localRotHistory.push_back(m_geo[i]->m_localRotation);
    if(i>0){
      translation += m_geo[i-1]->getTopTranslation();
    }
    else{
      translation.set(0,0,0);
    }
    m_transHistory.push_back(m_geo[i]->getTopTranslation());
  }
  if(m_rotHistory.size()/4 == Data::instance()->animLength){
    //exit(writeOut());
    m_buttons.clear();

    std::cout << "adding" << std::endl;

    ngl::Vec2 buttonPos;
    ngl::Vec2 buttonSize;
    float borderX = 50.0;
    float borderY = 50.0;

    borderX = borderX/m_width;
    borderY = borderY/m_height;

    float keyX = borderX * 2 - 1.0;
    float keyY = borderY * 2 - 1.0;

    float keyX2 = (keyX * -1);

    buttonPos.set(keyX, keyY);
    buttonSize.set(0.5, 0.55);

    if(Data::instance()->mode == Data::TOUCHSMALL_R){
      buttonPos.m_x *= -1;
      buttonPos.m_x -= buttonSize.m_x;
    }

    addButton(buttonPos, buttonSize, Data::instance()->baseColor, Button::Action::FINISH);

    updateButtonArrays();
    m_finished = true;
    m_timer2 = startTimer(1000/Data::instance()->fps);
  }
  else if(m_rotHistory.size()/4 > Data::instance()->animLength){
    std::cerr << "ERROR, size of history too large" << std::endl;
  }
  m_frame++;
  ngl::ShaderLib::instance()->use("progressShader");
  ngl::ShaderLib::instance()->setRegisteredUniform("frame", m_frame);
}
Ejemplo n.º 16
0
//--------------------------------------------------------------------------------------------------
/// Pan the camera up/down and left/right
/// 
/// \return  Returns true if input caused changes to the camera an false if no changes occurred
//--------------------------------------------------------------------------------------------------
bool ManipulatorTrackball::pan(int posX, int posY)
{
    if (m_camera.isNull()) return false;
    if (posX == m_lastPosX && posY == m_lastPosY) return false;

    const double vpPixSizeX = m_camera->viewport()->width();
    const double vpPixSizeY = m_camera->viewport()->height();
    if (vpPixSizeX <= 0 || vpPixSizeY <= 0) return false;

    // Normalized movement in screen plane 
    double tx = (posX - m_lastPosX)/vpPixSizeX;
    double ty = (posY - m_lastPosY)/vpPixSizeY;

    // Viewport size in world coordinates
    const double aspect = m_camera->aspectRatio();
    const double vpWorldSizeY = m_camera->frontPlaneFrustumHeight();
    const double vpWorldSizeX = vpWorldSizeY*aspect;

    const Vec3d camUp = m_camera->up();
    const Vec3d camRight = m_camera->right();


    Vec3d translation(0, 0, 0);

    Camera::ProjectionType projType = m_camera->projection();
    if (projType == Camera::PERSPECTIVE)
    {
        const Vec3d camPos = m_camera->position();
        const Vec3d camDir = m_camera->direction();
        const double nearPlane = m_camera->nearPlane();

        // Compute distance from camera to rotation point projected onto camera forward direction
        const Vec3d vDiff = m_rotationPoint - camPos;
        const double camRotPointDist = Math::abs(camDir*vDiff);

        Vec3d vX =  camRight*((tx*vpWorldSizeX)/nearPlane)*camRotPointDist;
        Vec3d vY =  camUp*((ty*vpWorldSizeY)/nearPlane)*camRotPointDist;
        translation = vX + vY;
    }
    else if (projType == Camera::ORTHO)
    {
        Vec3d vX =  camRight*tx*vpWorldSizeX;
        Vec3d vY =  camUp*ty*vpWorldSizeY;
        translation = vX + vY;
    }

    Mat4d viewMat = m_camera->viewMatrix();
    viewMat.translatePostMultiply(translation);
    m_camera->setViewMatrix(viewMat);

    m_lastPosX = posX;
    m_lastPosY = posY;

    return true;
}
Ejemplo n.º 17
0
void LifeCounter::Draw(mat4 view, mat4 projection) {
	projection = ortho(0.0f, 20.0f * (scene->width / (float)scene->height), 0.0f, 20.0f, -20.0f, 20.0f);
	for(int i=0; i<lives; i++) {
		Content::shader(basic).Begin();
			mat4 model = translation(position + vec3(i * icon.size.x, 0, 0))  * mat4_cast(orientation) * scale(size);
			model = model;
			glUniformMatrix4fv(Content::shader(basic)("inv_view"), 1, GL_FALSE, &(model[0][0]));
			icon.Draw(model, projection);
		Content::shader(basic).End();
	}
}
Ejemplo n.º 18
0
	STARTDECL(ph_render) ()
	{
		CheckPhysics();
		auto oldobject2view = object2view;
		auto oldcolor = curcolor;
		for (b2Body *body = world->GetBodyList(); body; body = body->GetNext())
		{
			auto pos = body->GetPosition();
			auto mat = translation(float3(pos.x, pos.y, 0)) * rotationZ(body->GetAngle());
			object2view = oldobject2view * mat;

			for (b2Fixture *fixture = body->GetFixtureList(); fixture; fixture = fixture->GetNext())
			{
				auto shapetype = fixture->GetType();
				auto r = (Renderable *)fixture->GetUserData();
				curcolor = r->color;
				r->Set();
				switch (shapetype)
				{
					case b2Shape::e_polygon:
					{
						auto polyshape = (b2PolygonShape *)fixture->GetShape();
						RenderArray(PRIM_FAN, polyshape->m_count, "pn", sizeof(b2Vec2), polyshape->m_vertices, NULL, 
							                                              sizeof(b2Vec2), polyshape->m_normals);
						break;
					}
					case b2Shape::e_circle:
					{
						// FIXME: instead maybe cache a circle verts somewhere.. though should maxverts be changable?
						const int maxverts = 20;
						struct PhVert { float2 pos; float2 norm; } phverts[maxverts];
						auto polyshape = (b2CircleShape *)fixture->GetShape();
						float step = PI * 2 / maxverts;
						for (int i = 0; i < maxverts; i++)
						{
							auto pos = float2(sinf(i * step + 1), cosf(i * step + 1));
							phverts[i].pos = pos * polyshape->m_radius + *(float2 *)&polyshape->m_p;
							phverts[i].norm = pos;
						}
						RenderArray(PRIM_FAN, maxverts, "pn", sizeof(PhVert), phverts, NULL);
						break;
					}
					case b2Shape::e_edge:
					case b2Shape::e_chain:
          case b2Shape::e_typeCount:
						assert(0);
						break;
				}
			}
		}
		object2view = oldobject2view;
		curcolor = oldcolor;
		return Value();
	}
Ejemplo n.º 19
0
void SimObject::setPosition(const Vector3d& pos)
{
  Vector3d translation(pos);
  translation -= position;
  position = pos;
  ObjectList::const_iterator listPos;
  for(listPos = childNodes.begin(); listPos != childNodes.end(); ++listPos)
  {
    (*listPos)->translate(translation);
  }
}
Ejemplo n.º 20
0
void arc_ball_camera::update(float deltaTime)
{
	glm::mat4 mat = glm::eulerAngleYX(_rotY, _rotX);
	glm::vec4 translation(0.0f, 0.0f, _distance, 1.0f);
	translation = mat * translation; 
	_position = _target + glm::vec3(translation.x, translation.y, translation.z);
	glm::vec4 tempUp(0.0f, 1.0f, 0.0f, 1.0f);
	tempUp = mat * tempUp;
	_up = glm::vec3(tempUp.x, tempUp.y, tempUp.z);
	_view = glm::lookAt(_position, _target, _up);
}
Ejemplo n.º 21
0
	Mat4 lookAt(const Vec3& eye, const Vec3& center, const Vec3& up)
	{
		Vec3 c_forward = (center - eye).normalize();
		Vec3 c_right = c_forward.cross(up).normalize();
		Vec3 c_up = c_right.cross(c_forward).normalize();

		return Mat4(c_right.x, c_up.x, -c_forward.x, 0,
			c_right.y, c_up.y, -c_forward.y, 0,
			c_right.z, c_up.z, -c_forward.z, 0,
			0, 0, 0, 1) * translation(-eye);
	}
Ejemplo n.º 22
0
/**
 * @brief First Estimation of the fitted shape by scaling only
 * @param iShape -- input shape
 * @param rect   -- the rectangle to calculate the scalar
 * @return VO_Shape -- the scaled shape
 */
VO_Shape VO_Fitting2DSM::VO_FirstEstimationByScaling(   const VO_Shape& iShape,
        const cv::Rect& rect )
{
    VO_Shape res = iShape;
    cv::Rect_<float> rect0 = iShape.GetShapeRect();
    float fScaleX = (float)rect.width/rect0.width *0.80;
    float fScaleY = (float)rect.height/rect0.height *0.80;
    res.ScaleX(fScaleX);
    res.ScaleY(fScaleY);
    rect0 = iShape.GetShapeBoundRect();
    cv::Mat_<float> translation = cv::Mat_<float>::zeros(2, 1);
    float centerX = (float)rect.x + (float)rect.width/2.0f;
    float centerY = (float)rect.y + (float)rect.height/2.0f;
    float center0X = (float)rect0.x + (float)rect0.width/2.0f;
    float center0Y = (float)rect0.x + (float)rect0.height/2.0f;
    translation(0,0) = centerX - center0X;
    translation(1,0) = centerY - center0Y;
    res.Translate( translation );
    return res;
}
Ejemplo n.º 23
0
void ShapeColliderTests::sphereMissesCapsule() {
    // non-overlapping sphere and capsule
    float radiusA = 1.5f;
    float radiusB = 2.3f;
    float totalRadius = radiusA + radiusB;
    float halfHeightB = 1.7f;
    float axialOffset = totalRadius + 1.1f * halfHeightB;
    float radialOffset = 1.2f * radiusA + 1.3f * radiusB;
    
    SphereShape sphereA(radiusA);
    CapsuleShape capsuleB(radiusB, halfHeightB);

    // give the capsule some arbirary transform
    float angle = 37.8f;
    glm::vec3 axis = glm::normalize( glm::vec3(-7.f, 2.8f, 9.3f) );
    glm::quat rotation = glm::angleAxis(angle, axis);
    glm::vec3 translation(15.1f, -27.1f, -38.6f);
    capsuleB.setRotation(rotation);
    capsuleB.setPosition(translation);

    CollisionList collisions(16);

    // walk sphereA along the local yAxis next to, but not touching, capsuleB
    glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f);
    int numberOfSteps = 10;
    float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1);
    for (int i = 0; i < numberOfSteps; ++i) {
        // translate sphereA into world-frame
        glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis;
        sphereA.setPosition(rotation * localPosition + translation);

        // sphereA agains capsuleB
        if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
        {
            std::cout << __FILE__ << ":" << __LINE__
                << " ERROR: sphere and capsule should NOT touch"
                << std::endl;
        }

        // capsuleB against sphereA
        if (ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
        {
            std::cout << __FILE__ << ":" << __LINE__
                << " ERROR: sphere and capsule should NOT touch"
                << std::endl;
        }
    }

    if (collisions.size() > 0) {
        std::cout << __FILE__ << ":" << __LINE__
            << " ERROR: expected empty collision list but size is " << collisions.size()
            << std::endl;
    }
}
Ejemplo n.º 24
0
Matrix2d Matrix2d::transformWith2P(const Point2d& from1, const Point2d& from2,
                                   const Point2d& to1, const Point2d& to2)
{
    if (from1 == from2 || to1 == to2
        || from1.isDegenerate() || from2.isDegenerate() || to1.isDegenerate() || to2.isDegenerate()) {
        return Matrix2d::kIdentity();
    }
    return (translation(to1 - from1)
            * scaling(to2.distanceTo(to1) / from2.distanceTo(from1), to1)
            * rotation((to2 - to1).angle2() - (from2 - from1).angle2(), to1));
}
Ejemplo n.º 25
0
//\fn void ExtrinsicParam::changePanTilt(double pan, double tilt);
///\brief This function computes the new rotation matrix and the new translation vector of the extrinsic parameters when the camera has changed its position.
///\param pan Value of the new camera panoramique.
///\param tilt Value of the new camera tilt.
void ExtrinsicParam::changePanTilt(double pan, double tilt)
{
  // Compute the rotation matrices with the new values of pan and tilt
  Eigen::Matrix3d Rx, Ry;
  Rx.setZero();
  Ry.setZero();
  Rx(0,0) = 1;
  Rx(1,1) = cos((-(tilt-this->tilt))*PI/180.);
  Rx(1,2) = -sin((-(tilt-this->tilt))*PI/180.);
  Rx(2,1) = sin((-(tilt-this->tilt))*PI/180.);
  Rx(2,2) = cos((-(tilt-this->tilt))*PI/180.);
  Ry(0,0) = cos((-(pan-this->pan))*PI/180.);
  Ry(0,2) = sin((-(pan-this->pan))*PI/180.);
  Ry(1,1) = 1;
  Ry(2,0) = -sin((-(pan-this->pan))*PI/180.);
  Ry(2,2) = cos((-(pan-this->pan))*PI/180.);

  Eigen::Vector3d Tx, Ty;
  Tx.setZero();
  Ty.setZero();
  Tx(0) = 2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos(0.5*(this->pan-pan)*PI/180.);
  Tx(2) = -2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos((90.-0.5*(this->pan-pan))*PI/180.);
  Ty(1) = 2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos(0.5*(this->tilt-tilt)*PI/180.);
  Ty(2) = -2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos((90.-0.5*(this->tilt-tilt))*PI/180.);

  // Compute the new values of the extrinsic parameters
  Eigen::Matrix4d Rx1, Ry1, Rt;
  Rt << initial_rotation, initial_translation,
    0,0,0,1;
  Rx1 << Rx, Tx,
    0,0,0,1;
  Ry1 << Ry, Ty,
    0,0,0,1;
  Rt.noalias() = Rx1*Ry1*Rt;
  rotation(0,0) = Rt(0,0);rotation(0,1) = Rt(0,1);rotation(0,2) = Rt(0,2);
  rotation(1,0) = Rt(1,0);rotation(1,1) = Rt(1,1);rotation(1,2) = Rt(1,2);
  rotation(2,0) = Rt(2,0);rotation(2,1) = Rt(2,1);rotation(2,2) = Rt(2,2);
  translation(0) = Rt(0,3);
  translation(1) = Rt(1,3);
  translation(2) = Rt(2,3);
}
Ejemplo n.º 26
0
	Matrix44f Camera::GetInverse() const
	{
		Matrix44f tempMatrix = myTransformation;
		Vector4f translation(tempMatrix.GetPosition());
		tempMatrix.SetPosition(Vector3f(0.0f, 0.0f, 0.0f));
		translation *= -1;
		translation.w = 1;
		tempMatrix.Transpose();
		translation = translation * tempMatrix;
		tempMatrix.SetPosition(Vector3f(translation.x, translation.y, translation.z));
		return tempMatrix;
	}
Ejemplo n.º 27
0
void SceneGraph::link(TransformInstance child, TransformInstance parent)
{
	unlink(child);

	if (!is_valid(_data.first_child[parent.i]))
	{
		_data.first_child[parent.i] = child;
		_data.parent[child.i] = parent;
	}
	else
	{
		TransformInstance prev = { UINT32_MAX };
		TransformInstance node = _data.first_child[parent.i];
		while (is_valid(node))
		{
			prev = node;
			node = _data.next_sibling[node.i];
		}

		_data.next_sibling[prev.i] = child;

		_data.first_child[child.i].i = UINT32_MAX;
		_data.next_sibling[child.i].i = UINT32_MAX;
		_data.prev_sibling[child.i] = prev;
	}

	Matrix4x4 parent_tr = _data.world[parent.i];
	Matrix4x4 child_tr = _data.world[child.i];
	const Vector3 cs = scale(child_tr);

	Vector3 px = x(parent_tr);
	Vector3 py = y(parent_tr);
	Vector3 pz = z(parent_tr);
	Vector3 cx = x(child_tr);
	Vector3 cy = y(child_tr);
	Vector3 cz = z(child_tr);

	set_x(parent_tr, normalize(px));
	set_y(parent_tr, normalize(py));
	set_z(parent_tr, normalize(pz));
	set_x(child_tr, normalize(cx));
	set_y(child_tr, normalize(cy));
	set_z(child_tr, normalize(cz));

	const Matrix4x4 rel_tr = child_tr * get_inverted(parent_tr);

	_data.local[child.i].position = translation(rel_tr);
	_data.local[child.i].rotation = to_matrix3x3(rel_tr);
	_data.local[child.i].scale = cs;
	_data.parent[child.i] = parent;

	transform(parent_tr, child);
}
Ejemplo n.º 28
0
const CU::Matrix33<float> Camera2D::GetInverse() const
{
	CU::Vector3<float> translation(-myOrientation.myMatrix[6], -myOrientation.myMatrix[7], 1);
	CU::Matrix33<float> inverse(myOrientation);
	inverse.myMatrix[6] = 0;
	inverse.myMatrix[7] = 0;
	inverse = CU::Transpose(inverse);
	translation *= inverse;
	inverse.myMatrix[6] = translation.x;
	inverse.myMatrix[7] = translation.y;
	return inverse;
}
Ejemplo n.º 29
0
// Normalize a blob using blob transformations. Less accurate, but
// more accurately copies the old way.
void DENORM::LocalNormBlob(TBLOB* blob) const {
  TBOX blob_box = blob->bounding_box();
  ICOORD translation(-IntCastRounded(x_origin_), -IntCastRounded(y_origin_));
  blob->Move(translation);
  if (y_scale_ != 1.0f)
    blob->Scale(y_scale_);
  if (rotation_ != NULL)
    blob->Rotate(*rotation_);
  translation.set_x(IntCastRounded(final_xshift_));
  translation.set_y(IntCastRounded(final_yshift_));
  blob->Move(translation);
}
Ejemplo n.º 30
0
void callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose_msf)//,
//                    const geometry_msgs::PoseStamped::ConstPtr &pose_sfm)
{

  // Extract translation and rotation
  Eigen::Vector3f translation(pose_msf->pose.pose.position.x, 
                              pose_msf->pose.pose.position.y, 
                              pose_msf->pose.pose.position.z);
  Eigen::Quaternionf q(pose_msf->pose.pose.orientation.w, 
                       pose_msf->pose.pose.orientation.x, 
                       pose_msf->pose.pose.orientation.y, 
                       pose_msf->pose.pose.orientation.z);
  
  img.copyTo(img_draw);
  draw(q, translation, cv::Scalar(255, 0, 0), tail); 
   
  draw(q_sfm, t_sfm, cv::Scalar(200, 200, 200), tail_sfm, false);
/*
  int c = img_cam.cols;
  int r = img_cam.rows;
  int cd = img_draw.cols;
  int rd = img_draw.rows;

  cv::Mat img_cam_r;
  cv::resize(img_cam, img_cam_r, cv::Size(rd * c / r, rd));

//  std::cout << img_cam_r.size() << std::endl;
//  std::cout << img_draw.size() << std::endl;

  cv::Mat img_sbs(img_draw.rows, img_draw.cols + img_cam_r.cols, CV_32FC3);
//  img_draw.convertTo(img_draw, CV_32FC3);
//  img_cam_r.convertTo(img_cam_r, CV_32FC3);

  for (size_t c = 0; c < img_draw.cols; ++c)
  {
    img_sbs.col(c) = img_draw.col(c);
  }


  for (size_t c = 0; c < img_cam_r.cols; ++c)
  {
    img_sbs.col(c + img_draw.cols) = img_cam_r.col(c);
  }
*/
  cv::resize(img_draw, img_draw, cv::Size(), 0.7, 0.7);

 // cv::namedWindow("2d-1");
  cv::namedWindow("2d-2");
 // cv::imshow("2d-1", img_cam_r);
  cv::imshow("2d-2", img_draw);

  cv::waitKey(1);
}