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

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

  ViewVC Help
Powered by ViewVC 1.1.22