aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs198
1 files changed, 198 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
new file mode 100644
index 0000000..5425a05
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
@@ -0,0 +1,198 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX.LinearMath
27{
28 internal class Quaternion : QuadWord
29 {
30 public Quaternion() { }
31
32 public Quaternion(float x, float y, float z, float w)
33 : base(x, y, z, w) { }
34
35 public Quaternion(Vector3 axis, float angle)
36 {
37 SetRotation(axis, angle);
38 }
39
40 public Quaternion(float yaw, float pitch, float roll)
41 {
42 SetEuler(yaw, pitch, roll);
43 }
44
45 public void SetRotation(Vector3 axis, float angle)
46 {
47 float d = axis.Length();
48 if (d == 0) throw new DivideByZeroException();
49 float s = (float)Math.Sin(angle * 0.5f) / d;
50 X = axis.X * s;
51 Y = axis.Y * s;
52 Z = axis.Z * s;
53 W = (float)Math.Cos(angle * 0.5f);
54 }
55
56 public void SetEuler(float yaw, float pitch, float roll)
57 {
58 float halfYaw = yaw * 0.5f;
59 float halfPitch = pitch * 0.5f;
60 float halfRoll = roll * 0.5f;
61 float cosYaw = (float)Math.Cos(halfYaw);
62 float sinYaw = (float)Math.Sin(halfYaw);
63 float cosPitch = (float)Math.Cos(halfPitch);
64 float sinPitch = (float)Math.Sin(halfPitch);
65 float cosRoll = (float)Math.Cos(halfRoll);
66 float sinRoll = (float)Math.Sin(halfRoll);
67 X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
68 Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
69 Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
70 W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
71 }
72
73 public float LengthSquared()
74 {
75 return Dot(this, this);
76 }
77
78 public float Length()
79 {
80 return (float)Math.Sqrt(LengthSquared());
81 }
82
83 public float Angle()
84 {
85 return 2f * (float)Math.Acos(W);
86 }
87
88 public static float Angle(Quaternion a, Quaternion b)
89 {
90 float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
91 if (s == 0) throw new DivideByZeroException();
92 return (float)Math.Acos(Dot(a, b) / s);
93 }
94
95 public static Quaternion Farthest(Quaternion a, Quaternion b)
96 {
97 Quaternion diff, sum;
98 diff = a - b;
99 sum = a + b;
100 if (Dot(diff, diff) > Dot(sum, sum))
101 return b;
102 return -b;
103 }
104
105 public static Quaternion Slerp(Quaternion a, Quaternion b, float c)
106 {
107 float theta = Angle(a, b);
108 if (theta != 0)
109 {
110 float d = 1f / (float)Math.Sin(theta);
111 float s0 = (float)Math.Sin((1f - c) * theta);
112 float s1 = (float)Math.Sin(c * theta);
113 return new Quaternion(
114 (a.X * s0 + b.X * s1) * d,
115 (a.Y * s0 + b.Y * s1) * d,
116 (a.Z * s0 + b.Z * s1) * d,
117 (a.W * s0 + b.W * s1) * d);
118 }
119 else
120 {
121 return a;
122 }
123 }
124
125 public static Quaternion Inverse(Quaternion a)
126 {
127 return new Quaternion(a.X, a.Y, a.Z, -a.W);
128 }
129
130 public static Quaternion Normalize(Quaternion a)
131 {
132 return a / a.Length();
133 }
134
135 public static float Dot(Quaternion a, Quaternion b)
136 {
137 return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
138 }
139
140 public static Quaternion operator +(Quaternion a, Quaternion b)
141 {
142 return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W);
143 }
144
145 public static Quaternion operator -(Quaternion a, Quaternion b)
146 {
147 return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W);
148 }
149
150 public static Quaternion operator -(Quaternion a)
151 {
152 return new Quaternion(-a.X, -a.Y, -a.Z, -a.W);
153 }
154
155 public static Quaternion operator *(Quaternion a, float b)
156 {
157 return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b);
158 }
159
160 public static Quaternion operator *(Quaternion a, Quaternion b)
161 {
162 return new Quaternion(
163 a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
164 a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
165 a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X,
166 a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z);
167 }
168
169 public static Quaternion operator *(Quaternion a, Vector3 b)
170 {
171 return new Quaternion(
172 a.W * b.X + a.Y * b.Z - a.Z * b.Y,
173 a.W * b.Y + a.Z * b.X - a.X * b.Z,
174 a.W * b.Z + a.X * b.Y - a.Y * b.X,
175 -a.X * b.X - a.Y * b.Y - a.Z * b.Z);
176 }
177
178 public static Quaternion operator *(Vector3 w, Quaternion q)
179 {
180 return new Quaternion(
181 w.X * q.W + w.Y * q.Z - w.Z * q.Y,
182 w.Y * q.W + w.Z * q.X - w.X * q.Z,
183 w.Z * q.W + w.X * q.Y - w.Y * q.X,
184 -w.X * q.X - w.Y * q.Y - w.Z * q.Z);
185 }
186
187 public static Quaternion operator /(Quaternion a, float b)
188 {
189 if (b == 0) throw new DivideByZeroException();
190 return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b);
191 }
192
193 public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a)
194 {
195 return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W);
196 }
197 }
198}
generated by cgit v1.1 at 2024-09-18 01:07:49 +0000