File: matrix.d

package info (click to toggle)
projectl 1.001.dfsg1-7
  • links: PTS, VCS
  • area: main
  • in suites: jessie, jessie-kfreebsd
  • size: 1,720 kB
  • ctags: 5
  • sloc: xml: 57; makefile: 32
file content (163 lines) | stat: -rw-r--r-- 6,361 bytes parent folder | download | duplicates (6)
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
module util.matrix;
private import util.vector;
private import std.math;
private import util.cloneable;
public class Matrix:Cloneable{
	public double[4][4] m;
	public this(){
		for(int i = m.length - 1;0 <= i;i --){
			for(int j = m[i].length - 1;0 <= j;j --){
				if( i == j)m[i][j] = 1;
				else m[i][j] = 0;
			}
		}
	}
	public this(Matrix t){
		for(int i = m.length - 1;0 <= i;i --){
			for(int j = m[i].length - 1;0 <= j;j --){
				m[i][j] = t.m[i][j];
			}
		}
	}
	public Object clone(){
		return new Matrix(this);
	}
	public Matrix opMul(Matrix t) {
    Matrix result = new Matrix();
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				result.m[i][j] = m[i][0] * t.m[0][j] +  m[i][1] * t.m[1][j] 
					+ m[i][2] * t.m[2][j] + m[i][3] * t.m[3][j];
			}
		}
		return result;
  }
	public Matrix opMul(double mul) {
    Matrix result = new Matrix();
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				result.m[i][j] = m[i][j] * mul;
			}
		}
		return result;
  }
	public void opMulAssign(Matrix t) {
		double[4][4] temp;
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				temp[i][j]  = m[i][0] * t.m[0][j] +  m[i][1] * t.m[1][j] 
					+ m[i][2] * t.m[2][j] + m[i][3] * t.m[3][j];
			}
		}
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				m[i][j] = temp[i][j];
			}
		}
	}
}
public Matrix matZero(){
	Matrix result = new Matrix();
	for(int i = 0;i < 4;i ++){
		for(int j = 0;j < 4;j ++){
			result.m[i][j] = 0;
		}
	}
	return result;
}
public Matrix matRotate(double deg ,double x, double y ,double z){
	Vector3 u = vec3Normalize(new Vector3(x ,y ,z));
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	double[4][4] UU;
	UU[0][0] = u.x * u.x;UU[0][1] = u.x * u.y;UU[0][2] = u.x * u.z;UU[0][3] =  0;
	UU[1][0] = u.y * u.x;UU[1][1] = u.y * u.y;UU[1][2] = u.y * u.z;UU[1][3] =  0;
	UU[2][0] = u.z * u.x;UU[2][1] = u.z * u.y;UU[2][2] = u.z * u.z;UU[2][3] =  0;
	UU[3][0] =  0;			 UU[3][1] =  0;				UU[3][2] =  0;			 UU[3][3] =  0;
	double[4][4] T ;
	T[0][0] = cos + (1 - cos) * UU[0][0]; T[0][1] = (1 - cos) * UU[0][1] - u.z * sin; T[0][2] = (1 - cos) * UU[0][2] + u.y * sin;T[0][3] = 0;
	T[1][0] = (1 - cos) * UU[1][0] + u.z * sin; T[1][1] = cos + (1 - cos) * UU[1][1]; T[1][2] = (1 - cos) * UU[1][2] - u.x * sin;T[1][3] = 0;
	T[2][0] = (1 - cos) * UU[2][0] - u.y * sin; T[2][1] = (1 - cos) * UU[2][1] + u.x * sin; T[2][2] = cos + (1 - cos) * UU[2][2];T[2][3] = 0;
	T[3][0] = 0																; T[3][1] = 0																; T[3][2] = 0													;T[3][3] = 1;
	for(int i = 0;i < 4;i ++){
		for(int j = 0;j < 4;j ++){
			result.m[i][j] = T[i][j];
		}
	}
	return result;
}
public Matrix matRotateX(double deg){
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	result.m[0][0] = 1;   result.m[0][1] = 0;    result.m[0][2] = 0;   	result.m[0][3] = 0;
	result.m[1][0] = 0;   result.m[1][1] = cos;  result.m[1][2] = sin; 	result.m[1][3] = 0;
	result.m[2][0] = 0;   result.m[2][1] = -sin; result.m[2][2] = cos; 	result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0;   	result.m[3][3] = 1;
	return result;
}

public Matrix matRotateY(double deg){
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	result.m[0][0] = cos; result.m[0][1] = 0;    result.m[0][2] = -sin;	result.m[0][3] = 0;
	result.m[1][0] = 0;   result.m[1][1] = 1;  	 result.m[1][2] = 0;		result.m[1][3] = 0;
	result.m[2][0] = sin; result.m[2][1] = 0; 	 result.m[2][2] = cos; 	result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0;   	result.m[3][3] = 1;
	return result;
}
public Matrix matRotateZ(double deg){
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	result.m[0][0] = cos; result.m[0][1] = sin; result.m[0][2] = 0; result.m[0][3] = 0;
	result.m[1][0] = -sin; result.m[1][1] = cos;  result.m[1][2] = 0; result.m[1][3] = 0;
	result.m[2][0] = 0;   result.m[2][1] = 0; 	 result.m[2][2] = 1; result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0; result.m[3][3] = 1;
	return result;
}
public Matrix matRotateXYZ(double degx ,double degy ,double degz){
	Matrix result = new Matrix();
	double sx = sin(degx * PI / 180.0);
	double sy = sin(degy * PI / 180.0);
	double sz = sin(degz * PI / 180.0);
	double cx = cos(degx * PI / 180.0);
	double cy = cos(degy * PI / 180.0);
	double cz = cos(degz * PI / 180.0);
	result.m[0][0] = cy*cz; result.m[0][1] = cy*sz; result.m[0][2] = -sy; result.m[0][3] = 0;
	result.m[1][0] = sx*sy*cz-cx*sz; result.m[1][1] = sx*sy*sz+cx*cz;  result.m[1][2] = sx*cy; result.m[1][3] = 0;
	result.m[2][0] = cx*sy*cz+sx*sz;   result.m[2][1] = cx*sy*sz-sx*cz; 	 result.m[2][2] = cx*cy; result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0; result.m[3][3] = 1;
	return result;
}
public Matrix matTranslate(double x,double y,double z){
	Matrix result = new Matrix();
	result.m[0][0] = 1; result.m[0][1] = 0;  result.m[0][2] = 0; 	result.m[0][3] = 0;
	result.m[1][0] = 0;	result.m[1][1] = 1;  result.m[1][2] = 0; 	result.m[1][3] = 0;
	result.m[2][0] = 0; result.m[2][1] = 0;  result.m[2][2] = 1; 	result.m[2][3] = 0;
	result.m[3][0] = x; result.m[3][1] = y;  result.m[3][2] = z;  result.m[3][3] = 1;
	return result;
}
public Matrix matScale(double sx,double sy,double sz){
	Matrix result = new Matrix();
	result.m[0][0] = sx;result.m[0][1] = 0;  result.m[0][2] = 0; 	result.m[0][3] = 0;
	result.m[1][0] = 0;	result.m[1][1] = sy; result.m[1][2] = 0; 	result.m[1][3] = 0;
	result.m[2][0] = 0; result.m[2][1] = 0;  result.m[2][2] = sz; result.m[2][3] = 0;
	result.m[3][0] = 0; result.m[3][1] = 0;  result.m[3][2] = 0;  result.m[3][3] = 1;
	return result;
}
public Vector3 vec3translate(Vector3 v,Matrix t){
	Vector3 result;
	double[4] v4 ,r4;
	v4[0] = v.x;v4[1] = v.y;v4[2] = v.z;v4[3] = 1;
	for(int i = 0;i < 4;i ++){
		r4[i] = 0;
		for(int j = 0;j < 4;j ++){
			r4[i] += v4[j] * t.m[j][i];
		}
	}
	result = new Vector3(r4[0] ,r4[1] ,r4[2]);
	return result;
}