こちらのプログラムを3D-2D変換したいです

前提・実現したいこと

Java(swing)で粒子法によるシミュレーションを行なっています。
こちらのプログラムで3D空間の座標(x, y, z)を2次元座標(x2d, y2d)に変換しマウスで視点を移動させて、好きな方向から観察できるようにしたいです。

該当のソースコード

Java
1import java.awt.geom.Point2D;
2import java.util.Arrays;
3import javax.swing.JFrame;
4
5class Main {
6    public static void main(String args[]) {
7        JFrame frame = new JFrame();
8        frame.setSize(500, 500);
9        frame.getContentPane().add(new MyJPanel());
10        frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
11        frame.setVisible(true);
12    }
13
14}
15
16class Point3D {
17  public double x, y, z;
18
19  Point3D(double x, double y, double z) {
20    this.x = x; this.y = y; this.z = z;
21  }
22}
23
24
25class MyJPanel extends javax.swing.JPanel {
26
27    /** 粒子の座標 */
28    final private Point3D[] m;
29    /** 重心の座標 */
30    final private Point3D g;
31    /** 角速度 */
32    Point3D omega;
33    /** 並進速度 */
34    Point3D v;
35
36    /**
37     * コンストラクタ
38     * <p>各粒子位置、重心、速度の初期化。および座標更新タイマの設定</p>
39     */
40    public MyJPanel() {
41                
42        // 粒子の座標を決める
43        m = new Point3D[] {
44                new Point3D(50, 50, 50), // m1
45                new Point3D(50, 70, 50), // m2
46                new Point3D(70, 70, 50), // m3
47        };
48
49        // 重心を計算(粒子の重さが全て等しい場合、座標平均が重心となる)
50        g = new Point3D(
51            Arrays.stream(m).mapToDouble(p -> p.x).average().getAsDouble(),
52            Arrays.stream(m).mapToDouble(p -> p.y).average().getAsDouble(),
53            Arrays.stream(m).mapToDouble(p -> p.z).average().getAsDouble());
54        
55        double m1x = 50, m1y = 50, m1z = 50, m2x = 50, m2y = 70, m2z = 50, m3x = 70, m3y = 70, m3z = 50; 
56        double derutat = 0.01;
57        double mg = 1;
58
59        double [][] am1 = new double[3][3];
60        am1[0][0] = mg * ((m1y - g.y) * (m1y - g.y) + (m1z - g.z) * (m1z - g.z));
61        am1[0][1] = mg * (-1 * (m1x - g.x) * (m1y - g.y));
62        am1[0][2] = mg * (-1 * (m1x - g.x) * (m1z - g.z));
63        am1[1][0] = mg * (-1 * (m1x - g.x) * (m1y - g.y));
64        am1[1][1] = mg * ((m1x - g.x) * (m1x - g.x) + (m1z - g.z) * (m1z - g.z));
65        am1[1][2] = mg * (-1 * (m1y - g.y) * (m1z - g.z));
66        am1[2][0] = mg * (-1 * (m1x - g.x) * (m1z - g.z));
67        am1[2][1] = mg * (-1 * (m1y - g.y) * (m1z - g.z));
68        am1[2][2] = mg * ((m1x - g.x) * (m1x - g.x) + (m1y - g.y) * (m1y - g.y));
69
70        double [][] am2 = new double[3][3];
71        am2[0][0] = mg * ((m2y - g.y) * (m2y - g.y) + (m2z - g.z) * (m2z - g.z));
72        am2[0][1] = mg * (-1 * (m2x - g.x) * (m2y - g.y));
73        am2[0][2] = mg * (-1 * (m2x - g.x) * (m2z - g.z));
74        am2[1][0] = mg * (-1 * (m2x - g.x) * (m2y - g.y));
75        am2[1][1] = mg * ((m2x - g.x) * (m2x - g.x) + (m2z - g.z) * (m2z - g.z));
76        am2[1][2] = mg * (-1 * (m2y - g.y) * (m2z - g.z));
77        am2[2][0] = mg * (-1 * (m2x - g.x) * (m2z - g.z));
78        am2[2][1] = mg * (-1 * (m2y - g.y) * (m2z - g.z));
79        am2[2][2] = mg * ((m2x - g.x) * (m2x - g.x) + (m2y - g.y) * (m2y - g.y));
80
81        double [][] am3 = new double[3][3];
82        am3[0][0] = mg * ((m3y - g.y) * (m3y - g.y) + (m3z - g.z) * (m3z - g.z));
83        am3[0][1] = mg * (-1 * (m3x - g.x) * (m3y - g.y));
84        am3[0][2] = mg * (-1 * (m3x - g.x) * (m3z - g.z));
85        am3[1][0] = mg * (-1 * (m3x - g.x) * (m3y - g.y));
86        am3[1][1] = mg * ((m3x - g.x) * (m3x - g.x) + (m3z - g.z) * (m3z - g.z));
87        am3[1][2] = mg * (-1 * (m3y - g.y) * (m3z - g.z));
88        am3[2][0] = mg * (-1 * (m3x - g.x) * (m3z - g.z));
89        am3[2][1] = mg * (-1 * (m3y - g.y) * (m3z - g.z));
90        am3[2][2] = mg * ((m3x - g.x) * (m3x - g.x) + (m3y - g.y) * (m3y - g.y));
91
92
93        double [][] a = new double[3][3];
94        a[0][0] = am1[0][0] + am2[0][0] + am3[0][0];
95        a[0][1] = am1[0][1] + am2[0][1] + am3[0][1];
96        a[0][2] = am1[0][2] + am2[0][2] + am3[0][2];
97        a[1][0] = am1[1][0] + am2[1][0] + am3[1][0];
98        a[1][1] = am1[1][1] + am2[1][1] + am3[1][1];
99        a[1][2] = am1[1][2] + am2[1][2] + am3[1][2];
100        a[2][0] = am1[2][0] + am2[2][0] + am3[2][0];
101        a[2][1] = am1[2][1] + am2[2][1] + am3[2][1];
102        a[2][2] = am1[2][2] + am2[2][2] + am3[2][2];
103
104        double [] k = new double[1];
105        k[0] = 1 / (a[0][0] * a[1][1] * a[2][2] + a[0][1] * a[1][2] * a[2][0] + a[0][2] * a[1][0] * a[2][1] - a[0][2] * a[1][1] * a[2][0] - a[0][1] * a[1][0] * a[2][2] - a[0][0] * a[1][2] * a[2][1]);
106
107        double [][] b = new double[3][3];
108        b[0][0] = k[0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]);
109        b[0][1] = k[0] * -1 * (a[0][1] * a[2][2] - a[0][2] * a[2][1]);
110        b[0][2] = k[0] * (a[0][1] * a[1][2] - a[0][2] * a[1][1]);
111        b[1][0] = k[0] * -1 * (a[1][0] * a[2][2] - a[1][2] * a[2][0]);
112        b[1][1] = k[0] * (a[0][0] * a[2][2] - a[0][2] * a[2][0]);
113        b[1][2] = k[0] * -1 * (a[0][0] * a[1][2] - a[0][2] * a[1][0]);
114        b[2][0] = k[0] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]);
115        b[2][1] = k[0] * -1 * (a[0][0] * a[2][1] - a[0][1] * a[2][0]);
116        b[2][2] = k[0] * (a[0][0] * a[1][1] - a[0][1] * a[1][0]);
117
118        double [] r = new double[3];
119        r[0] = m1x - g.x;
120        r[1] = m1y - g.y;
121        r[2] = m1z - g.z;
122
123        double [] f = new double[3];
124        f[0] = 500;
125        f[1] = 200;
126        f[2] = 200;
127
128        double [] gaiseki = new double[3];
129        gaiseki[0] = (r[1] * f[2] - r[2] * f[1]) * derutat;
130        gaiseki[1] = (r[2] * f[0] - r[0] * f[2]) * derutat;
131        gaiseki[2] = (r[0] * f[1] - r[1] * f[0]) * derutat;
132
133        double [] kakusokudo = new double[3];
134        kakusokudo[0] = b[0][0] * gaiseki[0] + b[0][1] * gaiseki[1] + b[0][2] * gaiseki[2];
135        kakusokudo[1] = b[1][0] * gaiseki[0] + b[1][1] * gaiseki[1] + b[1][2] * gaiseki[2];
136        kakusokudo[2] = b[2][0] * gaiseki[0] + b[2][1] * gaiseki[1] + b[2][2] * gaiseki[2];
137
138
139
140        // 角速度
141        omega = new Point3D(kakusokudo[0], kakusokudo[1], kakusokudo[2]);
142
143        // 並進速度
144        v = new Point3D(f[0] * derutat / (3 * mg), f[1] * derutat / (3 * mg), f[2] * derutat / (3 * mg) );
145 
146        // 30ミリ秒おきに位置座標を更新して、描画更新要求
147        new javax.swing.Timer(30, e -> {
148
149            // 並進運動による位置更新
150            g.x += v.x;
151            g.y += v.y;
152            g.z += v.z;
153
154            // 各粒子の位置更新
155            for (Point3D p : m) {
156                double x = p.x + v.x;
157                double y = p.y + v.y;
158                double z = p.z + v.z;
159                p.x = (x - g.x) * Math.cos(omega.y) * Math.cos(omega.z) + (y - g.y) * (Math.sin(omega.x) * Math.sin(omega.y) * Math.cos(omega.z) - Math.cos(omega.x) * Math.sin(omega.z)) + (z - g.z) * (Math.cos(omega.x) * Math.sin(omega.y) * Math.cos(omega.z) + Math.sin(omega.x) * Math.sin(omega.z)) + g.x;
160                p.y = (x - g.x) * Math.cos(omega.y) * Math.sin(omega.z) + (y - g.y) * (Math.sin(omega.x) * Math.sin(omega.y) * Math.sin(omega.z) + Math.cos(omega.x) * Math.cos(omega.z)) + (z - g.z) * (Math.cos(omega.x) * Math.sin(omega.y) * Math.sin(omega.z) - Math.sin(omega.x) * Math.cos(omega.z)) + g.y;
161                p.z = (x - g.x) * (-1) * Math.sin(omega.y) + (y - g.y) * Math.sin(omega.x) * Math.cos(omega.y) + (z - g.z) * Math.cos(omega.x) * Math.cos(omega.y) + g.z;
162            }          
163            // 描画更新要求
164            repaint();
165        }).start();
166    }
167
168    public void paintComponent(java.awt.Graphics g) {
169        // 粒子の描画
170        for(Point3D p : m) {
171            // とりえあえず、直径を20にしてる
172            g.drawOval((int)p.x, (int)p.y, 20, 20);
173        }
174
175    }
176}

試したこと

以下の式の正射影変換を用いることは分かったのですが、どこに、どの様に書けば良いのか分かりません。
x2d = px + xcos(beta) + zsin(beta)
zt = -xsin(beta) + zcos(beta)
y2d = py - (ycos(alpha) - ztsin(alpha)