template <typename PointT> pcl::PointCloud<PointT> projectToPlaneFromViewpoint (pcl::PointCloud<PointT>& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp) { Eigen::Vector3f norm (normal[0], normal[1], normal[2]); //(region.coefficients_[0], region.coefficients_[1], region.coefficients_[2]); pcl::PointCloud<PointT> projected_cloud; projected_cloud.resize (cloud.points.size ()); for (size_t i = 0; i < cloud.points.size (); i++) { Eigen::Vector3f pt (cloud.points[i].x, cloud.points[i].y, cloud.points[i].z); Eigen::Vector3f intersection = linePlaneIntersection (vp, pt, norm, centroid); projected_cloud[i].x = intersection[0]; projected_cloud[i].y = intersection[1]; projected_cloud[i].z = intersection[2]; } return (projected_cloud); }
void SampleMath::projectScreenPointToPlane(QCAR::Matrix44F inverseProjMatrix, QCAR::Matrix44F modelViewMatrix, float screenWidth, float screenHeight, QCAR::Vec2F point, QCAR::Vec3F planeCenter, QCAR::Vec3F planeNormal, QCAR::Vec3F &intersection, QCAR::Vec3F &lineStart, QCAR::Vec3F &lineEnd) { // Window Coordinates to Normalized Device Coordinates QCAR::VideoBackgroundConfig config = QCAR::Renderer::getInstance().getVideoBackgroundConfig(); float halfScreenWidth = screenWidth / 2.0f; float halfScreenHeight = screenHeight / 2.0f; float halfViewportWidth = config.mSize.data[0] / 2.0f; float halfViewportHeight = config.mSize.data[1] / 2.0f; float x = (point.data[0] - halfScreenWidth) / halfViewportWidth; float y = (point.data[1] - halfScreenHeight) / halfViewportHeight * -1; QCAR::Vec4F ndcNear(x, y, -1, 1); QCAR::Vec4F ndcFar(x, y, 1, 1); // Normalized Device Coordinates to Eye Coordinates QCAR::Vec4F pointOnNearPlane = Vec4FTransform(ndcNear, inverseProjMatrix); QCAR::Vec4F pointOnFarPlane = Vec4FTransform(ndcFar, inverseProjMatrix); pointOnNearPlane = Vec4FDiv(pointOnNearPlane, pointOnNearPlane.data[3]); pointOnFarPlane = Vec4FDiv(pointOnFarPlane, pointOnFarPlane.data[3]); // Eye Coordinates to Object Coordinates QCAR::Matrix44F inverseModelViewMatrix = Matrix44FInverse(modelViewMatrix); QCAR::Vec4F nearWorld = Vec4FTransform(pointOnNearPlane, inverseModelViewMatrix); QCAR::Vec4F farWorld = Vec4FTransform(pointOnFarPlane, inverseModelViewMatrix); lineStart = QCAR::Vec3F(nearWorld.data[0], nearWorld.data[1], nearWorld.data[2]); lineEnd = QCAR::Vec3F(farWorld.data[0], farWorld.data[1], farWorld.data[2]); linePlaneIntersection(lineStart, lineEnd, planeCenter, planeNormal, intersection); }