source: 2014/24/EemeliK/Zombieland/Jypeli/AdvanceMath/Vector3D.cs @ 5974

Revision 5974, 47.2 KB checked in by empaheik, 4 years ago (diff)
Line 
1#region MIT License
2/*
3 * Copyright (c) 2005-2008 Jonathan Mark Porter. http://physics2d.googlepages.com/
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights to
8 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
9 * the Software, and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
16 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
17 * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
18 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
20 * OTHER DEALINGS IN THE SOFTWARE.
21 */
22#endregion
23
24
25#if UseDouble
26using Scalar = System.Double;
27#else
28using Scalar = System.Single;
29#endif
30using System;
31using System.Runtime.InteropServices;
32using AdvanceMath.Design;
33using System.Xml.Serialization;
34namespace AdvanceMath
35{
36    /// <summary>
37    /// A Vector with 3 dimensions.
38    /// </summary>
39    /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29"/></remarks>
40    [StructLayout(LayoutKind.Sequential, Size = Vector3D.Size)]
41    [AdvBrowsableOrder("X,Y,Z")]
42#if !CompactFramework && !WindowsCE && !PocketPC && !XBOX360 && !SILVERLIGHT && !WINDOWS_PHONE
43    [Serializable]
44    [System.ComponentModel.TypeConverter(typeof(AdvTypeConverter<Vector3D>))]
45#endif
46    public struct Vector3D : IVector<Vector3D>
47    {
48        #region const fields
49        /// <summary>
50        /// The number of Scalar values in the class.
51        /// </summary>
52        public const int Count = 3;
53        /// <summary>
54        /// The Size of the class in bytes;
55        /// </summary>
56        public const int Size = sizeof(Scalar) * Count;
57        #endregion
58        #region readonly fields
59        /// <summary>
60        /// Vector3D(1,1,1)
61        /// </summary>
62        public static readonly Vector3D One = new Vector3D(1, 1, 1);
63        /// <summary>
64        /// Vector3D(0,0,0)
65        /// </summary>
66        public static readonly Vector3D Zero = new Vector3D();
67        /// <summary>
68        /// Vector3D(1,0,0)
69        /// </summary>
70        public static readonly Vector3D XAxis = new Vector3D(1, 0, 0);
71        /// <summary>
72        /// Vector3D(0,1,0)
73        /// </summary>
74        public static readonly Vector3D YAxis = new Vector3D(0, 1, 0);
75        /// <summary>
76        /// Vector3D(0,0,1)
77        /// </summary>
78        public static readonly Vector3D ZAxis = new Vector3D(0, 0, 1);
79
80        private static readonly string FormatString = MatrixHelper.CreateVectorFormatString(Count);
81        private readonly static string FormatableString = MatrixHelper.CreateVectorFormatableString(Count);
82        #endregion
83        #region static methods
84        public static void Copy(ref Vector3D vector, Scalar[] destArray)
85        {
86            Copy(ref vector, destArray, 0);
87        }
88        public static void Copy(ref Vector3D vector, Scalar[] destArray, int index)
89        {
90            ThrowHelper.CheckCopy(destArray, index, Count);
91
92            destArray[index] = vector.X;
93            destArray[++index] = vector.Y;
94            destArray[++index] = vector.Z;
95        }
96        public static void Copy(Scalar[] sourceArray, out Vector3D result)
97        {
98            Copy(sourceArray, 0, out result);
99        }
100        public static void Copy(Scalar[] sourceArray, int index, out Vector3D result)
101        {
102            ThrowHelper.CheckCopy(sourceArray, index, Count);
103
104            result.X = sourceArray[index];
105            result.Y = sourceArray[++index];
106            result.Z = sourceArray[++index];
107        }
108        public static void Copy(ref Vector4D source, out Vector3D dest)
109        {
110            dest.X = source.X;
111            dest.Y = source.Y;
112            dest.Z = source.Z;
113        }
114        public static void Copy(ref Vector2D source, ref Vector3D dest)
115        {
116            dest.X = source.X;
117            dest.Y = source.Y;
118        }
119
120        public static Vector3D Clamp(Vector3D value, Vector3D min, Vector3D max)
121        {
122            Vector3D result;
123            MathHelper.Clamp(ref value.X, ref  min.X, ref  max.X, out result.X);
124            MathHelper.Clamp(ref value.Y, ref  min.Y, ref  max.Y, out result.Y);
125            MathHelper.Clamp(ref value.Z, ref  min.Z, ref  max.Z, out result.Z);
126            return result;
127        }
128        public static void Clamp(ref Vector3D value, ref Vector3D min, ref Vector3D max, out Vector3D result)
129        {
130            MathHelper.Clamp(ref value.X, ref  min.X, ref  max.X, out result.X);
131            MathHelper.Clamp(ref value.Y, ref  min.Y, ref  max.Y, out result.Y);
132            MathHelper.Clamp(ref value.Z, ref  min.Z, ref  max.Z, out result.Z);
133        }
134
135        public static Vector3D Lerp(Vector3D left, Vector3D right, Scalar amount)
136        {
137            Vector3D result;
138            Lerp(ref left, ref right, ref amount, out result);
139            return result;
140        }
141        public static void Lerp(ref Vector3D left, ref Vector3D right, ref Scalar amount, out Vector3D result)
142        {
143            result.X = (right.X - left.X) * amount + left.X;
144            result.Y = (right.Y - left.Y) * amount + left.Y;
145            result.Z = (right.Z - left.Z) * amount + left.Z;
146        }
147        public static Vector3D Lerp(Vector3D left, Vector3D right, Vector3D amount)
148        {
149            Vector3D result;
150            Lerp(ref left, ref right, ref amount, out result);
151            return result;
152        }
153        public static void Lerp(ref Vector3D left, ref Vector3D right, ref Vector3D amount, out Vector3D result)
154        {
155            result.X = (right.X - left.X) * amount.X + left.X;
156            result.Y = (right.Y - left.Y) * amount.Y + left.Y;
157            result.Z = (right.Z - left.Z) * amount.Z + left.Z;
158        }
159
160        public static Scalar Distance(Vector3D left, Vector3D right)
161        {
162            Scalar result;
163            Distance(ref left, ref right, out result);
164            return result;
165        }
166        public static void Distance(ref Vector3D left, ref Vector3D right, out Scalar result)
167        {
168            Vector3D diff;
169            Subtract(ref left, ref right, out diff);
170            GetMagnitude(ref diff, out result);
171        }
172        public static Scalar DistanceSq(Vector3D left, Vector3D right)
173        {
174            Scalar result;
175            DistanceSq(ref left, ref right, out result);
176            return result;
177        }
178        public static void DistanceSq(ref Vector3D left, ref Vector3D right, out Scalar result)
179        {
180            Vector3D diff;
181            Subtract(ref left, ref right, out diff);
182            GetMagnitudeSq(ref diff, out result);
183        }
184
185        /// <summary>
186        /// Adds 2 Vectors2Ds.
187        /// </summary>
188        /// <param name="left">The left Vector3D operand.</param>
189        /// <param name="right">The right Vector3D operand.</param>
190        /// <returns>The Sum of the 2 Vector3Ds.</returns>
191        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Vector_addition_and_subtraction"/></remarks>
192        public static Vector3D Add(Vector3D left, Vector3D right)
193        {
194            Vector3D result;
195            result.X = left.X + right.X;
196            result.Y = left.Y + right.Y;
197            result.Z = left.Z + right.Z;
198            return result;
199        }
200        public static void Add(ref Vector3D left, ref Vector3D right, out Vector3D result)
201        {
202            result.X = left.X + right.X;
203            result.Y = left.Y + right.Y;
204            result.Z = left.Z + right.Z;
205        }
206
207        public static Vector3D Add(Vector2D left, Vector3D right)
208        {
209            Vector3D result;
210            Add(ref left, ref right, out result);
211            return result;
212        }
213        public static void Add(ref Vector2D left, ref Vector3D right, out Vector3D result)
214        {
215            result.X = left.X + right.X;
216            result.Y = left.Y + right.Y;
217            result.Z = right.Z;
218        }
219        public static Vector3D Add(Vector3D left, Vector2D right)
220        {
221            Vector3D result;
222            Add(ref left, ref right, out result);
223            return result;
224        }
225        public static void Add(ref Vector3D left, ref Vector2D right, out Vector3D result)
226        {
227            result.X = left.X + right.X;
228            result.Y = left.Y + right.Y;
229            result.Z = left.Z;
230        }
231
232
233        /// <summary>
234        /// Subtracts 2 Vector3Ds.
235        /// </summary>
236        /// <param name="left">The left Vector3D operand.</param>
237        /// <param name="right">The right Vector3D operand.</param>
238        /// <returns>The Difference of the 2 Vector3Ds.</returns>
239        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Vector_addition_and_subtraction"/></remarks>
240        public static Vector3D Subtract(Vector3D left, Vector3D right)
241        {
242            Vector3D result;
243            result.X = left.X - right.X;
244            result.Y = left.Y - right.Y;
245            result.Z = left.Z - right.Z;
246            return result;
247        }
248        public static void Subtract(ref Vector3D left, ref Vector3D right, out Vector3D result)
249        {
250            result.X = left.X - right.X;
251            result.Y = left.Y - right.Y;
252            result.Z = left.Z - right.Z;
253        }
254
255        public static Vector3D Subtract(Vector2D left, Vector3D right)
256        {
257            Vector3D result;
258            Subtract(ref left, ref right, out result);
259            return result;
260        }
261        public static void Subtract(ref Vector2D left, ref Vector3D right, out Vector3D result)
262        {
263            result.X = left.X - right.X;
264            result.Y = left.Y - right.Y;
265            result.Z = -right.Z;
266        }
267        public static Vector3D Subtract(Vector3D left, Vector2D right)
268        {
269            Vector3D result;
270            Subtract(ref left, ref right, out result);
271            return result;
272        }
273        public static void Subtract(ref Vector3D left, ref Vector2D right, out Vector3D result)
274        {
275            result.X = left.X - right.X;
276            result.Y = left.Y - right.Y;
277            result.Z = left.Z;
278        }
279
280        /// <summary>
281        /// Does Scaler Multiplication on a Vector3D.
282        /// </summary>
283        /// <param name="source">The Vector3D to be multiplied.</param>
284        /// <param name="scalar">The scalar value that will multiply the Vector3D.</param>
285        /// <returns>The Product of the Scaler Multiplication.</returns>
286        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Scalar_multiplication"/></remarks>
287        public static Vector3D Multiply(Vector3D source, Scalar scalar)
288        {
289            Vector3D result;
290            result.X = source.X * scalar;
291            result.Y = source.Y * scalar;
292            result.Z = source.Z * scalar;
293            return result;
294        }
295        public static void Multiply(ref Vector3D source, ref Scalar scalar, out Vector3D result)
296        {
297            result.X = source.X * scalar;
298            result.Y = source.Y * scalar;
299            result.Z = source.Z * scalar;
300        }
301        /// <summary>
302        /// Does Scaler Multiplication on a Vector3D.
303        /// </summary>
304        /// <param name="scalar">The scalar value that will multiply the Vector3D.</param>
305        /// <param name="source">The Vector3D to be multiplied.</param>
306        /// <returns>The Product of the Scaler Multiplication.</returns>
307        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Scalar_multiplication"/></remarks>
308        public static Vector3D Multiply(Scalar scalar, Vector3D source)
309        {
310            Vector3D result;
311            result.X = scalar * source.X;
312            result.Y = scalar * source.Y;
313            result.Z = scalar * source.Z;
314            return result;
315        }
316        public static void Multiply(ref Scalar scalar, ref Vector3D source, out Vector3D result)
317        {
318            result.X = scalar * source.X;
319            result.Y = scalar * source.Y;
320            result.Z = scalar * source.Z;
321        }
322        /// <summary>
323        ///             matrix * vector [3x3 * 3x1 = 3x1]
324        /// </summary>
325        /// <param name="vector"></param>
326        /// <param name="matrix"></param>
327        /// <returns></returns>
328        public static Vector3D Transform(Matrix3x3 matrix, Vector3D vector)
329        {
330            Vector3D result;
331
332            result.X = matrix.m00 * vector.X + matrix.m01 * vector.Y + matrix.m02 * vector.Z;
333            result.Y = matrix.m10 * vector.X + matrix.m11 * vector.Y + matrix.m12 * vector.Z;
334            result.Z = matrix.m20 * vector.X + matrix.m21 * vector.Y + matrix.m22 * vector.Z;
335
336            return result;
337        }
338        public static void Transform(ref Matrix3x3 matrix, ref Vector3D vector, out Vector3D result)
339        {
340            Scalar X = vector.X;
341            Scalar Y = vector.Y;
342            result.X = matrix.m00 * X + matrix.m01 * Y + matrix.m02 * vector.Z;
343            result.Y = matrix.m10 * X + matrix.m11 * Y + matrix.m12 * vector.Z;
344            result.Z = matrix.m20 * X + matrix.m21 * Y + matrix.m22 * vector.Z;
345        }
346        /// <summary>
347        ///             vector * matrix [1x3 * 3x3 = 1x3]
348        /// </summary>
349        /// <param name="vector"></param>
350        /// <param name="matrix"></param>
351        /// <returns></returns>
352        public static Vector3D Transform(Vector3D vector, Matrix3x3 matrix)
353        {
354            Vector3D result;
355
356            result.X = matrix.m00 * vector.X + matrix.m01 * vector.Y + matrix.m02 * vector.Z;
357            result.Y = matrix.m10 * vector.X + matrix.m11 * vector.Y + matrix.m12 * vector.Z;
358            result.Z = matrix.m20 * vector.X + matrix.m21 * vector.Y + matrix.m22 * vector.Z;
359
360            return result;
361        }
362        public static void Transform(ref Vector3D vector, ref Matrix3x3 matrix, out Vector3D result)
363        {
364            Scalar X = vector.X;
365            Scalar Y = vector.Y;
366            result.X = matrix.m00 * X + matrix.m01 * Y + matrix.m02 * vector.Z;
367            result.Y = matrix.m10 * X + matrix.m11 * Y + matrix.m12 * vector.Z;
368            result.Z = matrix.m20 * X + matrix.m21 * Y + matrix.m22 * vector.Z;
369        }
370        /// <summary>
371        ///             Transforms the given 3-D vector by the matrix, projecting the
372        ///             result back into <i>w</i> = 1.
373        ///             <p/>
374        ///             This means that the initial <i>w</i> is considered to be 1.0,
375        ///             and then all the tree elements of the resulting 3-D vector are
376        ///             divided by the resulting <i>w</i>.
377        /// </summary>
378        /// <param name="matrix">A Matrix4.</param>
379        /// <param name="vector">A Vector3D.</param>
380        /// <returns>A new vector.</returns>
381        public static Vector3D Transform(Matrix4x4 matrix, Vector3D vector)
382        {
383            Vector3D result;
384
385            Scalar inverseW = 1 / (matrix.m30 * vector.X + matrix.m31 * vector.Y + matrix.m32 * vector.Z + matrix.m33);
386            result.X = ((matrix.m00 * vector.X) + (matrix.m01 * vector.Y) + (matrix.m02 * vector.Z) + matrix.m03) * inverseW;
387            result.Y = ((matrix.m10 * vector.X) + (matrix.m11 * vector.Y) + (matrix.m12 * vector.Z) + matrix.m13) * inverseW;
388            result.Z = ((matrix.m20 * vector.X) + (matrix.m21 * vector.Y) + (matrix.m22 * vector.Z) + matrix.m23) * inverseW;
389
390            return result;
391        }
392        public static void Transform(ref Matrix4x4 matrix, ref Vector3D vector, out Vector3D result)
393        {
394            Scalar X = vector.X;
395            Scalar Y = vector.Y;
396            Scalar inverseW = 1 / (matrix.m30 * X + matrix.m31 * Y + matrix.m32 * vector.Z + matrix.m33);
397            result.X = ((matrix.m00 * X) + (matrix.m01 * Y) + (matrix.m02 * vector.Z) + matrix.m03) * inverseW;
398            result.Y = ((matrix.m10 * X) + (matrix.m11 * Y) + (matrix.m12 * vector.Z) + matrix.m13) * inverseW;
399            result.Z = ((matrix.m20 * X) + (matrix.m21 * Y) + (matrix.m22 * vector.Z) + matrix.m23) * inverseW;
400        }
401
402        public static Vector3D Multiply(Quaternion quat, Vector3D vector)
403        {
404            // nVidia SDK implementation
405            Vector3D uv, uuv;
406            Vector3D qvec;// = new Vector3D(quat.X, quat.Y, quat.Z);
407
408            qvec.X = quat.X;
409            qvec.Y = quat.Y;
410            qvec.Z = quat.Z;
411            Vector3D.Cross(ref qvec, ref vector, out uv);
412            Vector3D.Cross(ref qvec, ref uv, out uuv);
413            Vector3D.Cross(ref qvec, ref uv, out uuv);
414            Vector3D.Multiply(ref uv, ref quat.W, out uv);
415            Vector3D.Add(ref uv, ref uuv, out uv);
416            Vector3D.Multiply(ref uv, ref MathHelper.Two, out uv);
417            Vector3D.Add(ref vector, ref uv, out uv);
418            return uv;
419
420            //uv = qvec ^ vector;
421            //uuv = qvec ^ uv;
422            //uv *= (2 * quat.W);
423            //uuv *= 2;
424
425            //return vector + uv + uuv;
426
427            // get the rotation matriX of the Quaternion and multiplY it times the vector
428            //return quat.ToRotationMatrix() * vector;
429        }
430        public static void Multiply(ref Quaternion quat, ref Vector3D vector, out Vector3D result)
431        {
432            Vector3D uv, uuv;
433            Vector3D qvec;
434            qvec.X = quat.X;
435            qvec.Y = quat.Y;
436            qvec.Z = quat.Z;
437            Vector3D.Cross(ref qvec, ref vector, out uv);
438            Vector3D.Cross(ref qvec, ref uv, out uuv);
439            Vector3D.Cross(ref qvec, ref uv, out uuv);
440            Vector3D.Multiply(ref uv, ref quat.W, out uv);
441            Vector3D.Add(ref uv, ref uuv, out uv);
442            Vector3D.Multiply(ref uv, ref MathHelper.Two, out uv);
443            Vector3D.Add(ref vector, ref uv, out result);
444        }
445        /// <summary>
446        /// Does a Dot Operation Also know as an Inner Product.
447        /// </summary>
448        /// <param name="left">The left Vector3D operand.</param>
449        /// <param name="right">The right Vector3D operand.</param>
450        /// <returns>The Dot Product (Inner Product).</returns>
451        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Dot_product"/></remarks>
452        public static Scalar Dot(Vector3D left, Vector3D right)
453        {
454            return left.Y * right.Y + left.X * right.X + left.Z * right.Z;
455        }
456        public static void Dot(ref Vector3D left, ref Vector3D right, out Scalar result)
457        {
458            result = left.Y * right.Y + left.X * right.X + left.Z * right.Z;
459        }
460        /// <summary>
461        /// Does a Cross Operation Also know as an Outer Product.
462        /// </summary>
463        /// <param name="left">The left Vector3D operand.</param>
464        /// <param name="right">The right Vector3D operand.</param>
465        /// <returns>The Cross Product.</returns>
466        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Cross_product"/></remarks>
467        public static Vector3D Cross(Vector3D left, Vector3D right)
468        {
469            Vector3D result;
470            result.X = left.Y * right.Z - left.Z * right.Y;
471            result.Y = left.Z * right.X - left.X * right.Z;
472            result.Z = left.X * right.Y - left.Y * right.X;
473            return result;
474        }
475        public static void Cross(ref Vector3D left, ref Vector3D right, out Vector3D result)
476        {
477            Scalar X = left.Y * right.Z - left.Z * right.Y;
478            Scalar Y = left.Z * right.X - left.X * right.Z;
479            result.Z = left.X * right.Y - left.Y * right.X;
480            result.X = X;
481            result.Y = Y;
482        }
483        /// <summary>
484        /// Gets the Squared <see cref="Magnitude"/> of the Vector3D that is passed.
485        /// </summary>
486        /// <param name="source">The Vector3D whos Squared Magnitude is te be returned.</param>
487        /// <returns>The Squared Magnitude.</returns>
488        public static Scalar GetMagnitudeSq(Vector3D source)
489        {
490            return source.X * source.X + source.Y * source.Y + source.Z * source.Z;
491        }
492        public static void GetMagnitudeSq(ref Vector3D source, out Scalar result)
493        {
494            result = source.X * source.X + source.Y * source.Y + source.Z * source.Z;
495        }
496        /// <summary>
497        /// Gets the <see cref="Magnitude"/> of the Vector3D that is passed.
498        /// </summary>
499        /// <param name="source">The Vector3D whos Magnitude is te be returned.</param>
500        /// <returns>The Magnitude.</returns>
501        public static Scalar GetMagnitude(Vector3D source)
502        {
503            return MathHelper.Sqrt(source.X * source.X + source.Y * source.Y + source.Z * source.Z);
504        }
505        public static void GetMagnitude(ref Vector3D source, out Scalar result)
506        {
507            result = MathHelper.Sqrt(source.X * source.X + source.Y * source.Y + source.Z * source.Z);
508        }
509        /// <summary>
510        /// Sets the <see cref="Magnitude"/> of a Vector3D.
511        /// </summary>
512        /// <param name="source">The Vector3D whose Magnitude is to be changed.</param>
513        /// <param name="magnitude">The Magnitude.</param>
514        /// <returns>A Vector3D with the new Magnitude</returns>
515        public static Vector3D SetMagnitude(Vector3D source, Scalar magnitude)
516        {
517            Vector3D result;
518            SetMagnitude(ref source, ref magnitude, out result);
519            return result;
520        }
521        public static void SetMagnitude(ref Vector3D source, ref Scalar magnitude, out Vector3D result)
522        {
523            Scalar oldmagnitude;
524            GetMagnitude(ref source, out oldmagnitude);
525            if (oldmagnitude > 0 && magnitude != 0)
526            {
527                oldmagnitude = (magnitude / oldmagnitude);
528                Multiply(ref source, ref oldmagnitude, out result);
529            }
530            else
531            {
532                result = Zero;
533            }
534        }
535
536        /// <summary>
537        /// This returns the Normalized Vector3D that is passed. This is also known as a Unit Vector.
538        /// </summary>
539        /// <param name="source">The Vector3D to be Normalized.</param>
540        /// <returns>The Normalized Vector3D. (Unit Vector)</returns>
541        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Unit_vector"/></remarks>
542        public static Vector3D Normalize(Vector3D source)
543        {
544            Scalar oldmagnitude;
545            GetMagnitude(ref source, out oldmagnitude);
546            if (oldmagnitude == 0) { return Zero; }
547            oldmagnitude = (1 / oldmagnitude);
548            Vector3D result;
549            result.X = source.X * oldmagnitude;
550            result.Y = source.Y * oldmagnitude;
551            result.Z = source.Z * oldmagnitude;
552            return result;
553        }
554        public static void Normalize(ref Vector3D source, out Vector3D result)
555        {
556            Scalar oldmagnitude;
557            GetMagnitude(ref source, out oldmagnitude);
558            if (oldmagnitude == 0) { result = Zero; return; }
559            oldmagnitude = (1 / oldmagnitude);
560            result.X = source.X * oldmagnitude;
561            result.Y = source.Y * oldmagnitude;
562            result.Z = source.Z * oldmagnitude;
563        }
564        [CLSCompliant(false)]
565        public static void Normalize(ref Vector3D source)
566        {
567            Normalize(ref source, out source);
568        }
569        /// <summary>
570        /// Negates a Vector3D.
571        /// </summary>
572        /// <param name="source">The Vector3D to be Negated.</param>
573        /// <returns>The Negated Vector3D.</returns>
574        public static Vector3D Negate(Vector3D source)
575        {
576            Vector3D result;
577            result.X = -source.X;
578            result.Y = -source.Y;
579            result.Z = -source.Z;
580            return result;
581        }
582        [CLSCompliant(false)]
583        public static void Negate(ref Vector3D source)
584        {
585            Negate(ref source, out source);
586        }
587        public static void Negate(ref Vector3D source, out Vector3D result)
588        {
589            result.X = -source.X;
590            result.Y = -source.Y;
591            result.Z = -source.Z;
592        }
593
594        /// <summary>
595        /// Thie Projects the left Vector3D onto the Right Vector3D.
596        /// </summary>
597        /// <param name="left">The left Vector3D operand.</param>
598        /// <param name="right">The right Vector3D operand.</param>
599        /// <returns>The Projected Vector3D.</returns>
600        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Projection_%28linear_algebra%29"/></remarks>
601        public static Vector3D Project(Vector3D left, Vector3D right)
602        {
603            Vector3D result;
604            Project(ref left, ref right, out result);
605            return result;
606        }
607        public static void Project(ref Vector3D left, ref Vector3D right, out Vector3D result)
608        {
609            Scalar tmp, magsq;
610            Dot(ref left, ref  right, out tmp);
611            GetMagnitudeSq(ref right, out magsq);
612            tmp /= magsq;
613            Multiply(ref right, ref tmp, out result);
614        }
615
616        public static Vector3D Hermite(Vector3D value1, Vector3D tangent1, Vector3D value2, Vector3D tangent2, Scalar amount)
617        {
618            Vector3D result;
619            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
620            return result;
621        }
622        public static void Hermite(ref  Vector3D value1, ref Vector3D tangent1, ref Vector3D value2, ref Vector3D tangent2, Scalar amount, out Vector3D result)
623        {
624            Scalar h1, h2, h3, h4;
625            MathHelper.HermiteHelper(amount, out h1, out h2, out h3, out h4);
626            result.X = h1 * value1.X + h2 * value2.X + h3 * tangent1.X + h4 * tangent2.X;
627            result.Y = h1 * value1.Y + h2 * value2.Y + h3 * tangent1.Y + h4 * tangent2.Y;
628            result.Z = h1 * value1.Z + h2 * value2.Z + h3 * tangent1.Z + h4 * tangent2.Z;
629        }
630
631        public static Vector3D CatmullRom(Vector3D value1, Vector3D value2, Vector3D value3, Vector3D value4, Scalar amount)
632        {
633            Vector3D result;
634            CatmullRom(ref value1, ref value2, ref value3, ref value4, amount, out result);
635            return result;
636        }
637        public static void CatmullRom(ref Vector3D value1, ref Vector3D value2, ref Vector3D value3, ref Vector3D value4, Scalar amount, out Vector3D result)
638        {
639            Scalar amountSq = amount * amount;
640            Scalar amountCu = amountSq * amount;
641            result.X =
642                0.5f * ((2 * value2.X) +
643                (-value1.X + value3.X) * amount +
644                (2 * value1.X - 5 * value2.X + 4 * value3.X - value4.X) * amountSq +
645                (-value1.X + 3 * value2.X - 3 * value3.X + value4.X) * amountCu);
646            result.Y =
647                0.5f * ((2 * value2.Y) +
648                (-value1.Y + value3.Y) * amount +
649                (2 * value1.Y - 5 * value2.Y + 4 * value3.Y - value4.Y) * amountSq +
650                (-value1.Y + 3 * value2.Y - 3 * value3.Y + value4.Y) * amountCu);
651            result.Z =
652                0.5f * ((2 * value2.Z) +
653                (-value1.Z + value3.Z) * amount +
654                (2 * value1.Z - 5 * value2.Z + 4 * value3.Z - value4.Z) * amountSq +
655                (-value1.Z + 3 * value2.Z - 3 * value3.Z + value4.Z) * amountCu);
656        }
657
658        public static Vector3D Max(Vector3D value1, Vector3D value2)
659        {
660            Vector3D result;
661            Max(ref value1, ref value2, out result);
662            return result;
663        }
664        public static void Max(ref Vector3D value1, ref Vector3D value2, out Vector3D result)
665        {
666            result.X = (value1.X < value2.X) ? (value2.X) : (value1.X);
667            result.Y = (value1.Y < value2.Y) ? (value2.Y) : (value1.Y);
668            result.Z = (value1.Z < value2.Z) ? (value2.Z) : (value1.Z);
669        }
670
671        public static Vector3D Min(Vector3D value1, Vector3D value2)
672        {
673            Vector3D result;
674            Min(ref value1, ref value2, out result);
675            return result;
676        }
677        public static void Min(ref Vector3D value1, ref Vector3D value2, out Vector3D result)
678        {
679            result.X = (value1.X > value2.X) ? (value2.X) : (value1.X);
680            result.Y = (value1.Y > value2.Y) ? (value2.Y) : (value1.Y);
681            result.Z = (value1.Z > value2.Z) ? (value2.Z) : (value1.Z);
682        }
683
684        #endregion
685        #region fields
686        /// <summary>
687        /// This is the X value.
688        /// </summary>
689        [AdvBrowsable]
690        [XmlAttribute]
691        [System.ComponentModel.Description("The Magnitude on the X-Axis")]
692        public Scalar X;
693        /// <summary>
694        /// This is the Y value.
695        /// </summary>
696        [AdvBrowsable]
697        [XmlAttribute]
698        [System.ComponentModel.Description("The Magnitude on the Y-Axis")]
699        public Scalar Y;
700        /// <summary>
701        /// This is the Z value.
702        /// </summary>
703        [AdvBrowsable]
704        [XmlAttribute]
705        [System.ComponentModel.Description("The Magnitude on the Z-Axis")]
706        public Scalar Z;
707        #endregion
708        #region constructors
709        /// <summary>
710        /// Creates a New Vector3D Instance on the Stack.
711        /// </summary>
712        /// <param name="X">The X value.</param>
713        /// <param name="Y">The Y value.</param>
714        /// <param name="Z">The Z value.</param>
715        [InstanceConstructor("X,Y,Z")]
716        public Vector3D(Scalar X, Scalar Y, Scalar Z)
717        {
718            //this.Vector2D = Vector2D.Zero;
719            this.X = X;
720            this.Y = Y;
721            this.Z = Z;
722        }
723        public Vector3D(Scalar[] vals) : this(vals, 0) { }
724        public Vector3D(Scalar[] vals, int index)
725        {
726            Copy(vals, index, out this);
727        }
728
729        #endregion
730        #region indexers
731#if UNSAFE
732        /// <summary>
733        ///             Allows the Vector to be accessed linearly (v[0] -> v[Count-1]). 
734        /// </summary>
735        /// <remarks>
736        ///    This indexer is only provided as a convenience, and is <b>not</b> recommended for use in
737        ///    intensive applications. 
738        /// </remarks>
739        public Scalar this[int index]
740        {
741            get
742            {
743                ThrowHelper.CheckIndex("index", index, Count);
744                unsafe
745                {
746                    fixed (Scalar* ptr = &this.X)
747                    {
748                        return ptr[index];
749                    }
750                }
751            }
752            set
753            {
754                ThrowHelper.CheckIndex("index", index, Count);
755                unsafe
756                {
757                    fixed (Scalar* ptr = &this.X)
758                    {
759                        ptr[index] = value;
760                    }
761                }
762            }
763        }
764#endif
765        #endregion
766        #region public properties
767        /// <summary>
768        /// Gets or Sets the Magnitude (Length) of the Vector3D.
769        /// </summary>
770        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Length_of_a_vector"/></remarks>
771        [XmlIgnore]
772        public Scalar Magnitude
773        {
774            get
775            {
776                return MathHelper.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z);
777            }
778            set
779            {
780                this = SetMagnitude(this, value);
781            }
782        }
783        /// <summary>
784        /// Gets the Squared Magnitude of the Vector3D.
785        /// </summary>
786        public Scalar MagnitudeSq
787        {
788            get
789            {
790                return this.X * this.X + this.Y * this.Y + this.Z * this.Z;
791            }
792        }
793        /// <summary>
794        /// Gets the Normalized Vector3D. (Unit Vector)
795        /// </summary>
796        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Unit_vector"/></remarks>
797        public Vector3D Normalized
798        {
799            get
800            {
801                return Normalize(this);
802            }
803        }
804
805        /// <summary>
806        /// The Number of Variables accesable though the indexer.
807        /// </summary>
808        int IAdvanceValueType.Count { get { return Count; } }
809        #endregion
810        #region public methods
811        public Scalar[] ToArray()
812        {
813            Scalar[] array = new Scalar[Count];
814            Copy(ref this, array, 0);
815            return array;
816        }
817        public void CopyFrom(Scalar[] array, int index)
818        {
819            Copy(array, index, out this);
820        }
821        public void CopyTo(Scalar[] array, int index)
822        {
823            Copy(ref this, array, index);
824        }
825        #endregion
826        #region operators
827        /// <summary>
828        /// Adds 2 Vectors2Ds.
829        /// </summary>
830        /// <param name="left">The left Vector3D operand.</param>
831        /// <param name="right">The right Vector3D operand.</param>
832        /// <returns>The Sum of the 2 Vector3Ds.</returns>
833        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Vector_addition_and_subtraction"/></remarks>
834        public static Vector3D operator +(Vector3D left, Vector3D right)
835        {
836            Vector3D result;
837            result.X = left.X + right.X;
838            result.Y = left.Y + right.Y;
839            result.Z = left.Z + right.Z;
840            return result;
841        }
842        public static Vector3D operator +(Vector2D left, Vector3D right)
843        {
844            Vector3D result;
845            Add(ref left, ref right, out result);
846            return result;
847        }
848        public static Vector3D operator +(Vector3D left, Vector2D right)
849        {
850            Vector3D result;
851            Add(ref left, ref right, out result);
852            return result;
853        }
854        /// <summary>
855        /// Subtracts 2 Vector3Ds.
856        /// </summary>
857        /// <param name="left">The left Vector3D operand.</param>
858        /// <param name="right">The right Vector3D operand.</param>
859        /// <returns>The Difference of the 2 Vector3Ds.</returns>
860        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Vector_addition_and_subtraction"/></remarks>
861        public static Vector3D operator -(Vector3D left, Vector3D right)
862        {
863            Vector3D result;
864            result.X = left.X - right.X;
865            result.Y = left.Y - right.Y;
866            result.Z = left.Z - right.Z;
867            return result;
868        }
869        public static Vector3D operator -(Vector2D left, Vector3D right)
870        {
871            Vector3D result;
872            Subtract(ref left, ref right, out result);
873            return result;
874        }
875        public static Vector3D operator -(Vector3D left, Vector2D right)
876        {
877            Vector3D result;
878            Subtract(ref left, ref right, out result);
879            return result;
880        }
881        /// <summary>
882        /// Does Scaler Multiplication on a Vector3D.
883        /// </summary>
884        /// <param name="source">The Vector3D to be multiplied.</param>
885        /// <param name="scalar">The scalar value that will multiply the Vector3D.</param>
886        /// <returns>The Product of the Scaler Multiplication.</returns>
887        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Scalar_multiplication"/></remarks>
888        public static Vector3D operator *(Vector3D source, Scalar scalar)
889        {
890            Vector3D result;
891            result.X = source.X * scalar;
892            result.Y = source.Y * scalar;
893            result.Z = source.Z * scalar;
894            return result;
895        }
896        /// <summary>
897        /// Does Scaler Multiplication on a Vector3D.
898        /// </summary>
899        /// <param name="scalar">The scalar value that will multiply the Vector3D.</param>
900        /// <param name="source">The Vector3D to be multiplied.</param>
901        /// <returns>The Product of the Scaler Multiplication.</returns>
902        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Vector_%28spatial%29#Scalar_multiplication"/></remarks>
903        public static Vector3D operator *(Scalar scalar, Vector3D source)
904        {
905            Vector3D result;
906            result.X = scalar * source.X;
907            result.Y = scalar * source.Y;
908            result.Z = scalar * source.Z;
909            return result;
910        }
911        /// <summary>
912        /// Does a Dot Operation Also know as an Inner Product.
913        /// </summary>
914        /// <param name="left">The left Vector3D operand.</param>
915        /// <param name="right">The right Vector3D operand.</param>
916        /// <returns>The Dot Product (Inner Product).</returns>
917        /// <remarks><seealso href="http://en.wikipedia.org/wiki/Dot_product"/></remarks>
918        public static Scalar operator *(Vector3D left, Vector3D right)
919        {
920            return left.X * right.X + left.Y * right.Y + left.Z * right.Z;
921        }
922        /// <summary>
923        ///             Transforms the given 3-D vector by the matrix, projecting the
924        ///             result back into <i>w</i> = 1.
925        ///             <p/>
926        ///             This means that the initial <i>w</i> is considered to be 1.0,
927        ///             and then all the tree elements of the resulting 3-D vector are
928        ///             divided by the resulting <i>w</i>.
929        /// </summary>
930        /// <param name="matrix">A Matrix4.</param>
931        /// <param name="vector">A Vector3D.</param>
932        /// <returns>A new vector.</returns>
933        public static Vector3D operator *(Matrix4x4 matrix, Vector3D vector)
934        {
935            Vector3D result;
936
937            Scalar inverseW = 1 / (matrix.m30 * vector.X + matrix.m31 * vector.Y + matrix.m32 * vector.Z + matrix.m33);
938            result.X = ((matrix.m00 * vector.X) + (matrix.m01 * vector.Y) + (matrix.m02 * vector.Z) + matrix.m03) * inverseW;
939            result.Y = ((matrix.m10 * vector.X) + (matrix.m11 * vector.Y) + (matrix.m12 * vector.Z) + matrix.m13) * inverseW;
940            result.Z = ((matrix.m20 * vector.X) + (matrix.m21 * vector.Y) + (matrix.m22 * vector.Z) + matrix.m23) * inverseW;
941
942            return result;
943        }
944        /// <summary>
945        ///             matrix * vector [3x3 * 3x1 = 3x1]
946        /// </summary>
947        /// <param name="vector"></param>
948        /// <param name="matrix"></param>
949        /// <returns></returns>
950        public static Vector3D operator *(Matrix3x3 matrix, Vector3D vector)
951        {
952            Vector3D result;
953
954            result.X = matrix.m00 * vector.X + matrix.m01 * vector.Y + matrix.m02 * vector.Z;
955            result.Y = matrix.m10 * vector.X + matrix.m11 * vector.Y + matrix.m12 * vector.Z;
956            result.Z = matrix.m20 * vector.X + matrix.m21 * vector.Y + matrix.m22 * vector.Z;
957
958            return result;
959        }
960        public static Vector3D operator *(Quaternion quat, Vector3D vector)
961        {
962            // nVidia SDK implementation
963            Vector3D uv, uuv;
964            Vector3D qvec;// = new Vector3D(quat.X, quat.Y, quat.Z);
965
966            qvec.X = quat.X;
967            qvec.Y = quat.Y;
968            qvec.Z = quat.Z;
969            Cross(ref qvec, ref vector, out uv);
970            Cross(ref qvec, ref uv, out uuv);
971            Cross(ref qvec, ref uv, out uuv);
972            Multiply(ref uv, ref quat.W, out uv);
973            Add(ref uv, ref uuv, out uv);
974            Multiply(ref uv, ref MathHelper.Two, out uv);
975            Add(ref vector, ref uv, out uv);
976            return uv;
977
978            //uv = qvec ^ vector;
979            //uuv = qvec ^ uv;
980            //uv *= (2 * quat.W);
981            //uuv *= 2;
982
983            //return vector + uv + uuv;
984
985            // get the rotation matriX of the Quaternion and multiplY it times the vector
986            //return quat.ToRotationMatrix() * vector;
987        }
988        /// <summary>
989        ///             vector * matrix [1x3 * 3x3 = 1x3]
990        /// </summary>
991        /// <param name="vector"></param>
992        /// <param name="matrix"></param>
993        /// <returns></returns>
994        public static Vector3D operator *(Vector3D vector, Matrix3x3 matrix)
995        {
996            Vector3D result;
997
998            result.X = matrix.m00 * vector.X + matrix.m01 * vector.Y + matrix.m02 * vector.Z;
999            result.Y = matrix.m10 * vector.X + matrix.m11 * vector.Y + matrix.m12 * vector.Z;
1000            result.Z = matrix.m20 * vector.X + matrix.m21 * vector.Y + matrix.m22 * vector.Z;
1001
1002            return result;
1003        }
1004        /// <summary>
1005        /// Negates a Vector3D.
1006        /// </summary>
1007        /// <param name="source">The Vector3D to be Negated.</param>
1008        /// <returns>The Negated Vector3D.</returns>
1009        public static Vector3D operator -(Vector3D source)
1010        {
1011            Vector3D result;
1012            result.X = -source.X;
1013            result.Y = -source.Y;
1014            result.Z = -source.Z;
1015            return result;
1016        }
1017        /// <summary>
1018        /// Does a "2D" Cross Product also know as an Outer Product.
1019        /// </summary>
1020        /// <param name="left">The left Vector3D operand.</param>
1021        /// <param name="right">The right Vector3D operand.</param>
1022        /// <returns>The Z value of the resulting vector.</returns>
1023        /// <remarks>
1024        /// <seealso href="http://en.wikipedia.org/wiki/Cross_product"/>
1025        /// </remarks>
1026        public static Vector3D operator ^(Vector3D left, Vector3D right)
1027        {
1028            Vector3D result;
1029            result.X = left.Y * right.Z - left.Z * right.Y;
1030            result.Y = left.Z * right.X - left.X * right.Z;
1031            result.Z = left.X * right.Y - left.Y * right.X;
1032            return result;
1033        }
1034        /// <summary>
1035        /// Specifies whether the Vector3Ds contain the same coordinates.
1036        /// </summary>
1037        /// <param name="left">The left Vector3D to test.</param>
1038        /// <param name="right">The right Vector3D to test.</param>
1039        /// <returns>true if the Vector3Ds have the same coordinates; otherwise false</returns>
1040        public static bool operator ==(Vector3D left, Vector3D right)
1041        {
1042            return left.X == right.X && left.Y == right.Y && left.Z == right.Z;
1043        }
1044        /// <summary>
1045        /// Specifies whether the Vector3Ds do not contain the same coordinates.
1046        /// </summary>
1047        /// <param name="left">The left Vector3D to test.</param>
1048        /// <param name="right">The right Vector3D to test.</param>
1049        /// <returns>true if the Vector3Ds do not have the same coordinates; otherwise false</returns>
1050        public static bool operator !=(Vector3D left, Vector3D right)
1051        {
1052            return !(left.X == right.X && left.Y == right.Y && left.Z == right.Z);
1053        }
1054
1055        public static explicit operator Vector3D(Vector2D source)
1056        {
1057            Vector3D result;
1058            result.X = source.X;
1059            result.Y = source.Y;
1060            result.Z = 0;
1061            return result;
1062        }
1063
1064        public static explicit operator Vector3D(Vector4D source)
1065        {
1066            Vector3D result;
1067            result.X = source.X;
1068            result.Y = source.Y;
1069            result.Z = source.Z;
1070            return result;
1071        }
1072        #endregion
1073        #region overrides
1074        private string ToStringInternal(string FormatString)
1075        {
1076            return string.Format(FormatString, X, Y, Z);
1077        }
1078        public string ToString(string format)
1079        {
1080            return ToStringInternal(string.Format(FormatableString, format));
1081        }
1082        public override string ToString()
1083        {
1084            return ToStringInternal(FormatString);
1085        }
1086
1087#if !CompactFramework && !WindowsCE && !PocketPC && !XBOX360 && !SILVERLIGHT
1088        public static bool TryParse(string s, out Vector3D result)
1089        {
1090            if (s == null)
1091            {
1092                result = Zero;
1093                return false;
1094            }
1095            string[] vals = ParseHelper.SplitStringVector(s);
1096            if (vals.Length != Count)
1097            {
1098                result = Zero;
1099                return false;
1100            }
1101            if (Scalar.TryParse(vals[0], out result.X) &&
1102                Scalar.TryParse(vals[1], out result.Y) &&
1103                Scalar.TryParse(vals[2], out result.Z))
1104            {
1105                return true;
1106            }
1107            else
1108            {
1109                result = Zero;
1110                return false;
1111            }
1112        }
1113#endif
1114        [ParseMethod]
1115        public static Vector3D Parse(string s)
1116        {
1117            if (s == null)
1118            {
1119                throw new ArgumentNullException("s");
1120            }
1121            string[] vals = ParseHelper.SplitStringVector(s);
1122            if (vals.Length != Count)
1123            {
1124                ThrowHelper.ThrowVectorFormatException(s, Count, FormatString);
1125            }
1126            Vector3D value;
1127            value.X = Scalar.Parse(vals[0]);
1128            value.Y = Scalar.Parse(vals[1]);
1129            value.Z = Scalar.Parse(vals[2]);
1130            return value;
1131        }
1132
1133        /// <summary>
1134        ///             Provides a unique hash code based on the member variables of this
1135        ///             class.  This should be done because the equality operators (==, !=)
1136        ///             have been overriden by this class.
1137        ///             <p/>
1138        ///             The standard implementation is a simple XOR operation between all local
1139        ///             member variables.
1140        /// </summary>
1141        /// <returns></returns>
1142        public override int GetHashCode()
1143        {
1144            return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
1145        }
1146
1147        /// <summary>
1148        ///             Compares this Vector to another object.  This should be done because the
1149        ///             equality operators (==, !=) have been overriden by this class.
1150        /// </summary>
1151        /// <param name="obj"></param>
1152        /// <returns></returns>
1153        public override bool Equals(object obj)
1154        {
1155            return (obj is Vector3D) && Equals((Vector3D)obj);
1156        }
1157        public bool Equals(Vector3D other)
1158        {
1159            return Equals(ref this, ref other);
1160        }
1161        public static bool Equals(Vector3D left, Vector3D right)
1162        {
1163            return
1164                left.X == right.X &&
1165                left.Y == right.Y &&
1166                left.Z == right.Z;
1167        }
1168        [CLSCompliant(false)]
1169        public static bool Equals(ref Vector3D left, ref Vector3D right)
1170        {
1171            return
1172                left.X == right.X &&
1173                left.Y == right.Y &&
1174                left.Z == right.Z;
1175        }
1176        #endregion
1177    }
1178}
Note: See TracBrowser for help on using the repository browser.