C++ Transform3d::translate方法代码示例

Vec3d GLGizmoRotate::mouse_position_in_local_plane(const Linef3& mouse_ray, const Selection& selection) const
{
    double half_pi = 0.5 * (double)PI;

    Transform3d m = Transform3d::Identity();

    switch (m_axis)
    {
    case X:
    {
        m.rotate(Eigen::AngleAxisd(half_pi, Vec3d::UnitZ()));
        m.rotate(Eigen::AngleAxisd(-half_pi, Vec3d::UnitY()));
        break;
    }
    case Y:
    {
        m.rotate(Eigen::AngleAxisd(half_pi, Vec3d::UnitY()));
        m.rotate(Eigen::AngleAxisd(half_pi, Vec3d::UnitZ()));
        break;
    }
    default:
    case Z:
    {
        // no rotation applied
        break;
    }
    }

    if (selection.is_single_volume() || selection.is_single_modifier() || selection.requires_local_axes())
        m = m * selection.get_volume(*selection.get_volume_idxs().begin())->get_instance_transformation().get_matrix(true, false, true, true).inverse();

    m.translate(-m_center);

    return transform(mouse_ray, m).intersect_plane(0.0);
}

void rotateFrame(brics_3d::WorldModel* wm, unsigned int transformNodeId) {

	Eigen::Vector3d axis(1,1,1);
	axis.normalize();
	Eigen::AngleAxis<double> rotation(rotationValue, axis);
	//hen setting up an AngleAxis object, the axis vector must be normalized.
	Transform3d transformation;
	transformation = Eigen::Affine3d::Identity();
	transformation.translate(Eigen::Vector3d(0.15,0,0));
	transformation.rotate(rotation);
	brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr transform(new brics_3d::HomogeneousMatrix44(&transformation));

	wm->scene.setTransform(transformNodeId, transform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));

	rotationValue += 0.01;
}

void HomogeneousMatrixTest::testInverse() {
	HomogeneousMatrix44 pureTranslation(1,0,0, 0,1,0, 0,0,1, 1,2,3);

//	cout << "pureTranslation:" << endl << pureTranslation << endl;
	pureTranslation.inverse();
//	cout << "inverse of pureTranslation:" << endl << pureTranslation << endl;
	matrixPtr = pureTranslation.getRawData();

	CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, matrixPtr[12], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, matrixPtr[13], maxTolerance);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-3.0, matrixPtr[14], maxTolerance);

	HomogeneousMatrix44 pureTranslation2(1,0,0, 0,1,0, 0,0,1, 3,4,5);
	HomogeneousMatrix44 pureTranslation3(1,0,0, 0,1,0, 0,0,1, 3,4,5);
	pureTranslation3.inverse();

	IHomogeneousMatrix44* result = new HomogeneousMatrix44();
	result = pureTranslation2 * pureTranslation3;
	CPPUNIT_ASSERT(result->isIdentity() == true);


	/* now translation and rotation */

	AngleAxis<double> rotation(M_PI_2/4.0, Vector3d(1,0,0));
	Transform3d transformation;
	transformation = rotation;
	transformation.translate(Vector3d(5,6,99.9));
	IHomogeneousMatrix44* someTransform = new HomogeneousMatrix44(&transformation);
	IHomogeneousMatrix44* someOtherTransform = new HomogeneousMatrix44();
	*someOtherTransform = *someTransform;
	someTransform->inverse();
//	cout << "someTransform:" << endl << *someTransform << endl;

	result = (*someTransform) * (*someOtherTransform);
//	cout << "result:" << endl << *result << endl;
	CPPUNIT_ASSERT(result->isIdentity() == true);


}

void RoiManager::execute() {
	LOG(INFO) << "RoiManager: Adding a new ROI.";
	if (inputDataIds.size() < 1) {
		LOG(WARNING) << "RoiManager: Not enough input IDs.";
		return;
	}

	brics_3d::rsg::Id rootId = inputDataIds[0];

	Eigen::AngleAxis<double> rotation(roiPitch, Eigen::Vector3d(1,0,0));
	Transform3d transformation;
	transformation = Eigen::Affine3d::Identity();
	transformation.translate(Eigen::Vector3d(roiCenterX,roiCenterY,roiCenterZ));
	transformation.rotate(rotation);
	brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr transform(new brics_3d::HomogeneousMatrix44(&transformation));

	brics_3d::rsg::Id tfBox1Id = 0;
	brics_3d::rsg::Id Box1Id = 0;
	vector<brics_3d::rsg::Attribute> tmpAttributes;
	tmpAttributes.clear();
	tmpAttributes.push_back(Attribute("name","roi_box_tf"));
	wm->scene.addTransformNode(rootId, tfBox1Id, tmpAttributes, transform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));


	brics_3d::rsg::Box::BoxPtr box1(new brics_3d::rsg::Box(roiBoxSizeX, roiBoxSizeY, roiBoxSizeZ));
	tmpAttributes.clear();
	tmpAttributes.push_back(Attribute("name","roi_box"));
//	tmpAttributes.push_back(Attribute("debugInfo","no_visualization"));
	tmpAttributes.push_back(Attribute("rsgInfo","non_shared"));
	wm->scene.addGeometricNode(tfBox1Id, Box1Id, tmpAttributes, box1, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));
	LOG(DEBUG) << "ROI Box added with ID " << Box1Id;

	/* Here comes a basic robot skeleton */
	brics_3d::rsg::Id tfWorldToRobotId = 0;
	tmpAttributes.clear();
	tmpAttributes.push_back(Attribute("name","world_to_robot_tf"));
	brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr initialWorldToRobotTransform(new brics_3d::HomogeneousMatrix44());
	wm->scene.addTransformNode(wm->scene.getRootId(), tfWorldToRobotId, tmpAttributes, initialWorldToRobotTransform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));

	brics_3d::rsg::Id tfRobotToSensorId = 0;
	tmpAttributes.clear();
	tmpAttributes.push_back(Attribute("name","robot_to_sensor_tf"));
	brics_3d::HomogeneousMatrix44::IHomogeneousMatrix44Ptr initialRobotToSensorTransform(new brics_3d::HomogeneousMatrix44());
	LOG(DEBUG) << "current times stamp for frame = "<< timer.getCurrentTime();
	wm->scene.addTransformNode(tfWorldToRobotId, tfRobotToSensorId, tmpAttributes, initialRobotToSensorTransform, brics_3d::rsg::TimeStamp(timer.getCurrentTime()));



	/* We will add a hook for the processing data */
	brics_3d::rsg::Id sensorGroupId;
	tmpAttributes.clear();
	tmpAttributes.push_back(Attribute("name","sensor"));
	wm->scene.addGroup(tfRobotToSensorId, sensorGroupId, tmpAttributes);
	LOG(DEBUG) << "Sensor group added with ID " << sensorGroupId;

	/* We will add a hook for the scene objects */
	brics_3d::rsg::Id sceneObjectsGroupId;
	tmpAttributes.clear();
	tmpAttributes.push_back(Attribute("name","sceneObjects"));
	wm->scene.addGroup(wm->scene.getRootId(), sceneObjectsGroupId, tmpAttributes);
	LOG(DEBUG) << "Scene objects group  added with ID " << sensorGroupId;

}