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);
}
