1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
|
package ru.olamedia.math;
import static org.openmali.FastMath.*;
// 4x4 float matrix, column-major notation
public class Matrix4f {
protected float[] m;
public Matrix4f() {
m = new float[16];
}
public Matrix4f(float[] m) {
this.m = m;
}
public Matrix4f(javax.vecmath.Matrix4f m2) {
m = new float[16];
m[0] = m2.m00;
m[1] = m2.m10;
m[2] = m2.m20;
m[3] = m2.m30;
m[4] = m2.m01;
m[5] = m2.m11;
m[6] = m2.m21;
m[7] = m2.m31;
m[8] = m2.m02;
m[9] = m2.m12;
m[10] = m2.m22;
m[11] = m2.m32;
m[12] = m2.m03;
m[13] = m2.m13;
m[14] = m2.m23;
m[15] = m2.m33;
}
public float[] toFloatArray() {
return m;
}
public void set(int i, float v) {
m[i] = v;
}
public float get(int i) {
return m[i];
}
public void loadIdentity() {
setIdentity();
}
public void setIdentity() {
m[0] = m[5] = m[10] = m[15] = 1.0f;
m[1] = m[2] = m[3] = m[4] = 0.0f;
m[6] = m[7] = m[8] = m[9] = 0.0f;
m[11] = m[12] = m[13] = m[14] = 0.0f;
}
public static Matrix4f translateMatrix(float x, float y, float z) {
Matrix4f m = new Matrix4f();
m.setIdentity();
// Translate slots.
m.set(12, x);
m.set(13, y);
m.set(14, z);
return m;
}
public static Matrix4f scaleMatrix(float sx, float sy, float sz) {
Matrix4f m = new Matrix4f();
m.setIdentity();
// Scale slots.
m.set(0, sx);
m.set(5, sy);
m.set(10, sz);
return m;
}
public static Matrix4f rotateXMatrix(float degrees) {
float radians = toRad(degrees);
float c = cos(radians);
float s = sin(radians);
Matrix4f m = new Matrix4f();
m.setIdentity();
// Rotate X formula.
m.set(5, c);
m.set(6, -s);
m.set(9, s);
m.set(10, c);
return m;
}
public static Matrix4f rotateYMatrix(float degrees) {
float radians = toRad(degrees);
float c = cos(radians);
float s = sin(radians);
Matrix4f m = new Matrix4f();
m.setIdentity();
// Rotate Y formula.
m.set(0, c);
m.set(2, s);
m.set(8, -s);
m.set(10, c);
return m;
}
public static Matrix4f rotateZMatrix(float degrees) {
float radians = toRad(degrees);
float c = cos(radians);
float s = sin(radians);
Matrix4f m = new Matrix4f();
m.setIdentity();
// Rotate Z formula.
m.set(0, c);
m.set(1, s);
m.set(4, -s);
m.set(5, c);
return m;
}
public Vector3f getUpVector() {
return new Vector3f(m[1], m[5], m[9]);
}
public Vector3f getLookVector() { // POSITIVE_Z
return new Vector3f(m[2], m[6], m[10]);
}
public Vector3f getRightVector() {
return new Vector3f(m[0], m[4], m[8]);
}
public Matrix4f multiply(Matrix4f m) {
return Matrix4fUtil.multiply(this, m);
}
public void apply(Matrix4f m) {
this.m = multiply(m).toFloatArray();
}
public float[] getAngles() {
// TODO check majority
float ax, ay, az;
float cy;
ay = -asin(m[2]); /* Calculate Y-axis angle */
cy = cos(ay);
ay = toDeg(ay);
float trX, trY;
if (Math.abs(cy) > 0.005) /* Gimball lock? */
{
trX = m[10] / cy; /* No, so get X-axis angle */
trY = -m[6] / cy;
ax = toDeg(atan2(trY, trX));
trX = m[0] / cy; /* Get Z-axis angle */
trY = -m[1] / cy;
az = toDeg(atan2(trY, trX));
} else /* Gimball lock has occurred */
{
ax = 0; /* Set X-axis angle to zero */
trX = m[5]; /* And calculate Z-axis angle */
trY = m[4];
az = toDeg(atan2(trY, trX));
}
ax = clamp(ax, 0, 360); /* Clamp all angles to range */
ay = clamp(ay, 0, 360);
az = clamp(az, 0, 360);
return new float[] { ax, ay, ax };
}
private float clamp(float a, float min, float max) {
a = a % max;
return a;
}
public float c(int column, int row) {
// COLUMN-BASED
return m[column * 4 + row];
}
public void set(float[] m) {
this.m = m;
}
}
|
