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