/[gr2lib]/trunk/gr2lib/core/coretypes/implementation/Matrix44.cs
ViewVC logotype

Contents of /trunk/gr2lib/core/coretypes/implementation/Matrix44.cs

Parent Directory Parent Directory | Revision Log Revision Log


Revision 186 - (show annotations) (download)
Sat Jul 24 02:23:18 2010 UTC (9 years, 3 months ago) by william
File size: 14104 byte(s)
continue to add xml documentation

1 using System;
2 using System.Collections.Generic;
3 using System.Linq;
4 using System.Text;
5 using System.Runtime.InteropServices;
6
7 namespace gr2lib.core.coretypes.implementation
8 {
9 /// <summary>
10 /// Matrix44 strucure
11 /// </summary>
12 [StructLayout(LayoutKind.Sequential)]
13 public struct Matrix44
14 {
15 /// <summary>
16 /// right
17 /// </summary>
18 public Vector4 right;
19 /// <summary>
20 /// up
21 /// </summary>
22 public Vector4 up;
23 /// <summary>
24 /// at
25 /// </summary>
26 public Vector4 at;
27 /// <summary>
28 /// pos
29 /// </summary>
30 public Vector4 pos;
31 /// <summary>
32 /// Invalid
33 /// </summary>
34 public static readonly Matrix44 Invalid = new Matrix44(Vector4.Invalid, Vector4.Invalid, Vector4.Invalid, Vector4.Invalid);
35 /// <summary>
36 /// Identity
37 /// </summary>
38 public static readonly Matrix44 Identity = new Matrix44(new Vector4(1f, 0f, 0f, 0f), new Vector4(0f, 1f, 0f, 0f), new Vector4(0f, 0f, 1f, 0f), new Vector4(0f, 0f, 0f, 1f));
39
40
41 /// <summary>
42 /// default constructor: r,u,a,p
43 /// </summary>
44 /// <param name="r"></param>
45 /// <param name="u"></param>
46 /// <param name="a"></param>
47 /// <param name="p"></param>
48 public Matrix44(Vector4 r, Vector4 u, Vector4 a, Vector4 p)
49 {
50 this.right = r;
51 this.up = u;
52 this.at = a;
53 this.pos = p;
54 }
55 /// <summary>
56 /// default constructor: from matrix44
57 /// </summary>
58 /// <param name="m"></param>
59 internal Matrix44(native.Matrix44 m)
60 {
61 this.right = new Vector4(m.Right.X, m.Right.Y, m.Right.Z, m.Right.W);
62 this.up = new Vector4(m.Up.X, m.Up.Y, m.Up.Z, m.Up.W);
63 this.at = new Vector4(m.Back.X, m.Back.Y, m.Back.Z, m.Back.W);
64 this.pos = new Vector4(m.Pos.X, m.Pos.Y, m.Pos.Z, m.Pos.W);
65 }
66 /// <summary>
67 /// default constructor: from matrix33
68 /// </summary>
69 /// <param name="m"></param>
70 public Matrix44(Matrix33 m)
71 {
72 this.right = new Vector4(m.right.x, m.right.y, m.right.z, 0f);
73 this.up = new Vector4(m.up.x, m.up.y, m.up.z, 0f);
74 this.at = new Vector4(m.at.x, m.at.y, m.at.z, 0f);
75 this.pos = new Vector4(0f, 0f, 0f, 1f);
76 }
77 /// <summary>
78 /// SetEulerRotations(float rx, float ry, float rz)
79 /// </summary>
80 /// <param name="rx"></param>
81 /// <param name="ry"></param>
82 /// <param name="rz"></param>
83 public void SetEulerRotations(float rx, float ry, float rz)
84 {
85 float sX = (float)Math.Sin((double)rx);
86 float cX = (float)Math.Cos((double)rx);
87 float sY = (float)Math.Sin((double)ry);
88 float cY = (float)Math.Cos((double)ry);
89 float sZ = (float)Math.Sin((double)rz);
90 float cZ = (float)Math.Cos((double)rz);
91 this.right.Set((cY * cZ), -(cY * sZ * cX) + (sY * sX), (cY * sZ * sX) + (sY * cX), 0f);
92 this.up.Set(sZ, (cZ * cX), -(cZ * sX), 0f);
93 this.at.Set(-(sY * cZ), (sY * sZ * cX) + (cY * sX), -(sY * sZ * sX) + (cY * cX), 0f);
94 }
95 /// <summary>
96 /// SetIdentity()
97 /// </summary>
98 public void SetIdentity()
99 {
100 this.right.Set(1f, 0f, 0f, 0f);
101 this.up.Set(0f, 1f, 0f, 0f);
102 this.at.Set(0f, 0f, 1f, 0f);
103 this.pos.Set(0f, 0f, 0f, 1f);
104 }
105 /// <summary>
106 /// SetPerspective(float w, float h, float zn, float zf)
107 /// </summary>
108 /// <param name="w"></param>
109 /// <param name="h"></param>
110 /// <param name="zn"></param>
111 /// <param name="zf"></param>
112 public void SetPerspective(float w, float h, float zn, float zf)
113 {
114 this.right.Set((2f * zn) / w, 0f, 0f, 0f);
115 this.up.Set(0f, (2f * zn) / h, 0f, 0f);
116 this.at.Set(0f, 0f, zf / (zn - zf), -1f);
117 this.pos.Set(0f, 0f, (zn * zf) / (zn - zf), 0f);
118 }
119 /// <summary>
120 /// SetRotation(Quaternion q)
121 /// </summary>
122 /// <param name="q"></param>
123 public void SetRotation(Quaternion q)
124 {
125 float len = q.Magnitude;
126 q = q / len;
127 this.right.Set(1 - 2 * (q.v.y * q.v.y + q.v.z * q.v.z), 2 * (q.v.x * q.v.y - q.v.z * q.n), 2 * (q.v.x * q.v.z + q.v.y * q.n), 0f);
128 this.up.Set(2 * (q.v.x * q.v.y + q.v.z * q.n), 1 - 2 * (q.v.x * q.v.x + q.v.z * q.v.z), 2 * (q.v.y * q.v.z - q.v.x * q.n), 0f);
129 this.at.Set(2 * (q.v.x * q.v.z - q.v.y * q.n), 2 * (q.v.y * q.v.z + q.v.x * q.n), 1 - 2 * (q.v.x * q.v.x + q.v.y * q.v.y), 0f);
130 this.right *= len;
131 this.up *= len;
132 this.at *= len;
133 }
134 /// <summary>
135 /// SetRotation(Vector3 axis, float angle)
136 /// </summary>
137 /// <param name="axis"></param>
138 /// <param name="angle"></param>
139 public void SetRotation(Vector3 axis, float angle)
140 {
141 float len = axis.Length();
142 axis.Normalize();
143 float cos = (float)Math.Cos(angle);
144 float sin = (float)Math.Sin(angle);
145 this.right.Set(axis.x * axis.x + (1 - axis.x * axis.x) * cos, axis.x * axis.y * (1 - cos) - axis.z * sin, axis.x * axis.z * (1 - cos) + axis.y * sin, 0f);
146 this.up.Set(axis.x * axis.y * (1 - cos) + axis.z * sin, axis.y * axis.y + (1 - axis.y * axis.y) * cos, axis.y * axis.z * (1 - cos) - axis.x * sin, 0f);
147 this.at.Set(axis.x * axis.z * (1 - cos) - axis.y * sin, axis.y * axis.z * (1 - cos) + axis.x * sin, axis.z * axis.z + (1 - axis.z * axis.z) * cos, 0f);
148 this.right *= len;
149 this.up *= len;
150 this.at *= len;
151 }
152 /// <summary>
153 /// SetRotation(Vector4 raxis)
154 /// </summary>
155 /// <param name="raxis"></param>
156 public void SetRotation(Vector4 raxis)
157 {
158 SetRotation(raxis.V3, raxis.w);
159 }
160 /// <summary>
161 /// SetRotationX(float rads) -> could use Microsoft.Xna.Framework.Matrix.CreateRotationX(float radians)
162 /// </summary>
163 /// <param name="rads"></param>
164 public void SetRotationX(float rads)
165 {
166
167 float sin = (float)Math.Sin((double)rads);
168 float cos = (float)Math.Cos((double)rads);
169 this.right.Set(1f, 0f, 0f, 0f);
170 this.up.Set(0f, cos, -sin, 0f);
171 this.at.Set(0f, sin, cos, 0f);
172 }
173 /// <summary>
174 /// SetRotationY(float rads) -> could use Microsoft.Xna.Framework.Matrix.CreateRotationY(float radians)
175 /// </summary>
176 /// <param name="rads"></param>
177 public void SetRotationY(float rads)
178 {
179 float sin = (float)Math.Sin((double)rads);
180 float cos = (float)Math.Cos((double)rads);
181 this.right.Set(cos, 0f, sin, 0f);
182 this.up.Set(0f, 1f, 0f, 0f);
183 this.at.Set(-sin, 0f, cos, 0f);
184 }
185 /// <summary>
186 /// SetRotationZ(float rads) -> could use Microsoft.Xna.Framework.Matrix.CreateRotationZ(float radians)
187 /// </summary>
188 /// <param name="rads"></param>
189 public void SetRotationZ(float rads)
190 {
191 float sin = (float)Math.Sin((double)rads);
192 float cos = (float)Math.Cos((double)rads);
193 this.right.Set(cos, -sin, 0f, 0f);
194 this.up.Set(sin, cos, 0f, 0f);
195 this.at.Set(0f, 0f, 1f, 0f);
196 }
197
198 /// <summary>
199 /// double Magnitude()
200 /// </summary>
201 /// <returns></returns>
202 public double Magnitude()
203 {
204 return Math.Sqrt(this.right.LengthSqr() + this.up.LengthSqr() + this.at.LengthSqr() + this.pos.LengthSqr());
205 }
206
207 /// <summary>
208 /// Test if instances are equal
209 /// </summary>
210 /// <param name="obj"></param>
211 /// <returns></returns>
212 public override bool Equals(object obj)
213 {
214 if ((obj == null) || (obj.GetType() != base.GetType()))
215 {
216 return false;
217 }
218 Matrix44 matrix = (Matrix44)obj;
219 return (this.right.Equals(matrix.right) && this.up.Equals(matrix.up) && this.at.Equals(matrix.at) && this.pos.Equals(matrix.pos));
220 }
221 /// <summary>
222 /// Get Hash Code
223 /// </summary>
224 /// <returns></returns>
225 public override int GetHashCode()
226 {
227 return (this.right.GetHashCode() ^ this.up.GetHashCode() ^ this.at.GetHashCode() ^ this.pos.GetHashCode());
228 }
229
230 /// <summary>
231 /// Get string representation
232 /// </summary>
233 /// <returns></returns>
234 public override string ToString()
235 {
236 StringBuilder builder = new StringBuilder();
237 builder.Append("r:[");
238 builder.Append(this.right.ToString());
239 builder.Append("] u:[");
240 builder.Append(this.up.ToString());
241 builder.Append("] a:[");
242 builder.Append(this.at.ToString());
243 builder.Append("] p:[");
244 builder.Append(this.pos.ToString());
245 builder.Append("]");
246 return builder.ToString();
247 }
248
249 /// <summary>
250 /// ToQuaternion()
251 /// </summary>
252 /// <returns></returns>
253 public Quaternion ToQuaternion()
254 {
255 double mag = this.Magnitude();
256 double m00 = (double)this.right.x / mag;
257 double m11 = (double)this.up.y / mag;
258 double m22 = (double)this.at.z / mag;
259 double qx = Math.Sqrt(0.25 * (1.0 - m00 - m11 + m22)) * mag;
260 double qy = Math.Sqrt(0.25 * (1.0 - m00 + m11 - m22)) * mag;
261 double qz = Math.Sqrt(0.25 * (1.0 + m00 - m11 - m22)) * mag;
262 double[] qw = new double[6];
263 double m01 = (double)this.right.y / mag;
264 qw[0] = (0.5 * m01 - qx * qy) / qz * mag;
265 double m02 = (double)this.right.z / mag;
266 qw[1] = (qx * qz - 0.5 * m02) / qy * mag;
267 double m10 = (double)this.up.x / mag;
268 qw[2] = (qx * qy - 0.5 * m10) / qz * mag;
269 double m12 = (double)this.up.z / mag;
270 qw[3] = (0.5 * m12 - qy * qz) / qx * mag;
271 double m20 = (double)this.at.x / mag;
272 qw[4] = (0.5 * m20 - qx * qz) / qy * mag;
273 double m21 = (double)this.at.y / mag;
274 qw[5] = (qy * qz - 0.5 * m21) / qx * mag;
275 return new Quaternion((float)qx, (float)qy, (float)qz, (float)qw[1]);
276 }
277 /// <summary>
278 /// ToMatrix33()
279 /// </summary>
280 /// <returns></returns>
281 public Matrix33 ToMatrix33()
282 {
283 return new Matrix33(this.right.V3, this.up.V3, this.at.V3);
284 }
285
286 /// <summary>
287 /// DebugDrawMatrixTimed(float time) [UNUSED]
288 /// </summary>
289 /// <param name="time"></param>
290 public void DebugDrawMatrixTimed(float time)
291 {
292 }
293
294 /// <summary>
295 /// TransformVector(Vector3 vec)
296 /// </summary>
297 /// <param name="vec"></param>
298 /// <returns></returns>
299 public Vector3 TransformVector(Vector3 vec)
300 {
301 return (Vector3)(((this.right.V3 * vec.x) + (this.up.V3 * vec.y)) + (this.at.V3 * vec.z));
302 }
303 /// <summary>
304 /// InverseTransformPoint(Vector3 vec)
305 /// </summary>
306 /// <param name="vec"></param>
307 /// <returns></returns>
308 public Vector3 InverseTransformPoint(Vector3 vec)
309 {
310 return this.InverseTransformVector(vec - this.pos.V3);
311 }
312 /// <summary>
313 /// InverseTransformVector(Vector3 vec)
314 /// </summary>
315 /// <param name="vec"></param>
316 /// <returns></returns>
317 public Vector3 InverseTransformVector(Vector3 vec)
318 {
319 return new Vector3((float)(this.right.V3 * vec), (float)(this.up.V3 * vec), (float)(this.at.V3 * vec));
320 }
321
322 /// <summary>
323 /// CopyMatrix(Matrix44 obj)
324 /// </summary>
325 /// <param name="obj"></param>
326 /// <returns></returns>
327 public static Matrix44 CopyMatrix(Matrix44 obj)
328 {
329 Matrix44 copy = new Matrix44();
330 copy.pos = new Vector4(obj.pos.x, obj.pos.y, obj.pos.z, obj.pos.w);
331 copy.right = new Vector4(obj.right.x, obj.right.y, obj.right.z, obj.right.w);
332 copy.up = new Vector4(obj.up.x, obj.up.y, obj.up.z, obj.up.w);
333 copy.at = new Vector4(obj.at.x, obj.at.y, obj.at.z, obj.at.w);
334 return copy;
335 }
336 /// <summary>
337 /// operator *(Matrix44 a, Matrix44 b)
338 /// </summary>
339 /// <param name="a"></param>
340 /// <param name="b"></param>
341 /// <returns></returns>
342 public static Matrix44 operator *(Matrix44 a, Matrix44 b)
343 {
344 Matrix44 matrix = Matrix44.CopyMatrix(Identity);
345 matrix.right.V3 = (Vector3)(((b.right.V3 * a.right.V3.x) + (b.up.V3 * a.right.V3.y)) + (b.at.V3 * a.right.V3.z));
346 matrix.up.V3 = (Vector3)(((b.right.V3 * a.up.V3.x) + (b.up.V3 * a.up.V3.y)) + (b.at.V3 * a.up.V3.z));
347 matrix.at.V3 = (Vector3)(((b.right.V3 * a.at.V3.x) + (b.up.V3 * a.at.V3.y)) + (b.at.V3 * a.at.V3.z));
348 matrix.pos.V3 = ((Vector3)(((b.right.V3 * a.pos.V3.x) + (b.up.V3 * a.pos.V3.y)) + (b.at.V3 * a.pos.V3.z))) + b.pos.V3;
349 return matrix;
350 }
351 }
352 }

  ViewVC Help
Powered by ViewVC 1.1.22