3D Co-Ordinate system Transformation

I have 2 Points in 3D P1(x1,y1,z1) & P2(x2,y2,z2) with Origin at Origin(0,0,0).

Now I want to shift the Origin to P1(x1,y1,z1) with new Rotation vector R.

How to represent the Point P2(x2,y2,z2) with respect to the new Origin?

How to compute the Affine Transformation Matrix of Point P1 & Origin, so that the same Point P2 Can be represented with respect to P1 as Origin?

enter image description here

To Explain My problem I’ve added a simple 3D Simulation to verify my Result!

When there is No Rotation involved the Points are Exactly the same as shown in this Image. You can Notice that Both Red and Yellow Lines are overlapping in the Image.

enter image description here

But When there is some Rotation is involved in the New Origin, Then the Points are not matching!

enter image description here

Here is the code to reproduce the same results!

#include <iostream>
    #include <fstream>
    #include <string>
    #include <Windows.h>
    #include <gl/GL.h>
    #include <gl/GLU.h>
    #pragma comment( lib, "opengl32.lib" )  
    #pragma comment( lib, "glu32.lib" )  
    #include <opencv2/opencv.hpp>
    #include <opencv2/plot.hpp>
    #include <opencv2/viz.hpp>
    using namespace std;
    using namespace cv;
    int main(int argc)
        const int
            IMAGE_WIDTH = 1280,
            IMAGE_HEIGHT = 720;
        const double
            fx = IMAGE_WIDTH,
            fy = IMAGE_HEIGHT,
            cx = IMAGE_WIDTH / 2.0,
            cy = IMAGE_HEIGHT / 2.0;
        double camMatarray[9] = {
            fx, 0., cx,
            0., fy, cy,
            0., 0., 1.
        Matx33d CameraMatrix = Matx33d(
            fx, 0, cx,
            0, fy, cy,
            0, 0, 1);
        Point3d ptv_VehicleOrigin(0, 0, 0); //Origin
        Point3d ptV_Ball3D(4238, 0, 324.00);//Point P1
        Point3d ptV_Camera3D(3778, 53, 650);//Point P2
        Point3d ptV_Camera3D_Orientation(5, 197, 0);    //Not -Working Case
        //Point3d ptV_Camera3D_Orientation(0, 0, 0);    //Working Case
        /// Create 3D windows
        Affine3d cam_pose = Affine3d(Mat::eye(3, 3, CV_64F), ptV_Camera3D);
        viz::Viz3d Window_3D("World 3D Frame");
        Window_3D.setWindowSize(Size(IMAGE_WIDTH, IMAGE_HEIGHT));
        Window_3D.setBackgroundColor(); // black by default
        while (!Window_3D.wasStopped())
              Affine3d AffineTransform = Affine3d(Mat::eye(3, 3, CV_64F), ptV_Camera3D);
          //Apply Translation
          Point3d ptC_Ball3D = ptV_Ball3D - ptV_Camera3D;
          cout << "Ball 2 Camera Translation: " << ptC_Ball3D << "\n";
          //Apply Rotation
          Vec3f R = Vec3f((float)ptV_Camera3D_Orientation.x, (float)ptV_Camera3D_Orientation.y, (float)ptV_Camera3D_Orientation.z);
          Affine3d Rotation = Affine3d(eulerAnglesToRotationMatrix(R));
          ptC_Ball3D = Rotation * ptC_Ball3D;
          //Apply Rotation
          cout << "Ball 2 Camera Rotation: " << ptC_Ball3D << "\n";
            viz::WText3D TextBall(cv::format("Ball [%.0f,%.0f,%.0f]", ptV_Ball3D.x, ptV_Ball3D.y, ptV_Ball3D.z), ptV_Ball3D, 15.0);
            Window_3D.showWidget("Ball Text", TextBall);
            viz::WText3D TextCamera(cv::format("Camera [%.0f,%.0f,%.0f]", ptV_Camera3D.x, ptV_Camera3D.y, ptV_Camera3D.z), ptV_Camera3D, 15.0);
            Window_3D.showWidget("Camera Text", TextCamera);
            viz::WText3D TextBall2(cv::format("Ball2 [%.0f,%.0f,%.0f]", ptC_Ball3D.x, ptC_Ball3D.y, ptC_Ball3D.z), ptC_Ball3D, 15.0, false);
            Window_3D.showWidget("Camera Text1", TextBall2, AffineTransform);
            viz::WLine lineOrigin2Camera(ptv_VehicleOrigin, ptV_Camera3D, cv::viz::Color::blue());
            lineOrigin2Camera.setRenderingProperty(viz::LINE_WIDTH, 2.0);
            Window_3D.showWidget("Line Origin2Camera", lineOrigin2Camera);
            viz::WLine lineCamera2Ball(ptV_Camera3D, ptV_Ball3D, cv::viz::Color::yellow());
            lineCamera2Ball.setRenderingProperty(viz::LINE_WIDTH, 2.0);
            Window_3D.showWidget("Line Camera2Ball", lineCamera2Ball);
            viz::WLine lineOrigin2Ball(ptv_VehicleOrigin, ptV_Ball3D, cv::viz::Color::green());
            lineOrigin2Ball.setRenderingProperty(viz::LINE_WIDTH, 2.0);
            Window_3D.showWidget("Line Origin2Ball", lineOrigin2Ball);
            viz::WLine lineCamera2Ball_1(ptv_VehicleOrigin, ptC_Ball3D, cv::viz::Color::red());
            lineCamera2Ball_1.setRenderingProperty(viz::LINE_WIDTH, 2.0);
            Window_3D.showWidget("Line Camera2Ball1", lineCamera2Ball_1, AffineTransform);
            // Create a sphere widget
            viz::WSphere swBall(Point3d(ptV_Ball3D), 10.01f, 10, cv::viz::Color::blue());
            // Cast sphere widget to cloud widget
            viz::WCloud cwBall = swBall.cast<viz::WCloud>();
            /// Modify it, and it will be modified in the window.
            // Create a sphere widget
            viz::WSphere swCamera(ptV_Camera3D, 10.01f, 0.25, cv::viz::Color::yellow());
            // Cast sphere widget to cloud widget
            viz::WCloud cwCamera = swCamera.cast<viz::WCloud>();
            /// Modify it, and it will be modified in the window.
            // Display it in a window
            Window_3D.showWidget("SphereBall", cwBall);
            cam_pose = Affine3d(ptV_Camera3D_Orientation, ptV_Camera3D);
            Affine3d vehicle_pose = Affine3d(Mat::eye(3, 3, CV_64F), ptv_VehicleOrigin);
            viz::WCameraPosition cpw(500); // Coordinate axes
            viz::WCameraPosition cpw_frustum(CameraMatrix, 150, viz::Color::yellow()); // Camera frustum
            viz::WCameraPosition VehicleAxis(500); // Coordinate axes
            viz::WCameraPosition VehicleAxis_frustum(Matx33d::eye(), 150, viz::Color::yellow()); // Camera frustum
            Window_3D.showWidget("SphereCamera", cwCamera);
            Window_3D.showWidget("CPW", cpw, cam_pose);
            Window_3D.showWidget("VehicleAxis", VehicleAxis, vehicle_pose);
            Window_3D.spinOnce(100, true);
        return 0;

rotations can’t translate. your requirement is nonsensical. you have been told that much in your Stack Overflow question.

why post the same question again, with no reaction to the advice you were given?

Hi @crackwitz Thank you for your comments! From the stackoverflow comments it was Not clear to me that “Rotations cant translate”! That’s why I cross-posted here!

But my Original Intention was to Simulate the Position of Ball with respect to the World Co-Ordinate. Could you Please suggest How to verify the Transformation in the 3D?