aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs209
1 files changed, 209 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs
new file mode 100644
index 0000000..0ba8f17
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs
@@ -0,0 +1,209 @@
1/* The MIT License
2 *
3 * Copyright (c) 2010 Intel Corporation.
4 * All rights reserved.
5 *
6 * Based on the convexdecomposition library from
7 * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
8 *
9 * Permission is hereby granted, free of charge, to any person obtaining a copy
10 * of this software and associated documentation files (the "Software"), to deal
11 * in the Software without restriction, including without limitation the rights
12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 * copies of the Software, and to permit persons to whom the Software is
14 * furnished to do so, subject to the following conditions:
15 *
16 * The above copyright notice and this permission notice shall be included in
17 * all copies or substantial portions of the Software.
18 *
19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 * THE SOFTWARE.
26 */
27
28using System;
29
30namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
31{
32 public class Quaternion : float4
33 {
34 public Quaternion()
35 {
36 x = y = z = 0.0f;
37 w = 1.0f;
38 }
39
40 public Quaternion(float3 v, float t)
41 {
42 v = float3.normalize(v);
43 w = (float)Math.Cos(t / 2.0f);
44 v = v * (float)Math.Sin(t / 2.0f);
45 x = v.x;
46 y = v.y;
47 z = v.z;
48 }
49
50 public Quaternion(float _x, float _y, float _z, float _w)
51 {
52 x = _x;
53 y = _y;
54 z = _z;
55 w = _w;
56 }
57
58 public float angle()
59 {
60 return (float)Math.Acos(w) * 2.0f;
61 }
62
63 public float3 axis()
64 {
65 float3 a = new float3(x, y, z);
66 if (Math.Abs(angle()) < 0.0000001f)
67 return new float3(1f, 0f, 0f);
68 return a * (1 / (float)Math.Sin(angle() / 2.0f));
69 }
70
71 public float3 xdir()
72 {
73 return new float3(1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y));
74 }
75
76 public float3 ydir()
77 {
78 return new float3(2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x));
79 }
80
81 public float3 zdir()
82 {
83 return new float3(2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y));
84 }
85
86 public float3x3 getmatrix()
87 {
88 return new float3x3(xdir(), ydir(), zdir());
89 }
90
91 public static implicit operator float3x3(Quaternion q)
92 {
93 return q.getmatrix();
94 }
95
96 public static Quaternion operator *(Quaternion a, Quaternion b)
97 {
98 Quaternion c = new Quaternion();
99 c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
100 c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
101 c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
102 c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
103 return c;
104 }
105
106 public static float3 operator *(Quaternion q, float3 v)
107 {
108 // The following is equivalent to:
109 //return (q.getmatrix() * v);
110 float qx2 = q.x * q.x;
111 float qy2 = q.y * q.y;
112 float qz2 = q.z * q.z;
113
114 float qxqy = q.x * q.y;
115 float qxqz = q.x * q.z;
116 float qxqw = q.x * q.w;
117 float qyqz = q.y * q.z;
118 float qyqw = q.y * q.w;
119 float qzqw = q.z * q.w;
120 return new float3((1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z, (2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z, (2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z);
121 }
122
123 public static Quaternion operator +(Quaternion a, Quaternion b)
124 {
125 return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
126 }
127
128 public static Quaternion operator *(Quaternion a, float b)
129 {
130 return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b);
131 }
132
133 public static Quaternion normalize(Quaternion a)
134 {
135 float m = (float)Math.Sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z);
136 if (m < 0.000000001f)
137 {
138 a.w = 1;
139 a.x = a.y = a.z = 0;
140 return a;
141 }
142 return a * (1f / m);
143 }
144
145 public static float dot(Quaternion a, Quaternion b)
146 {
147 return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
148 }
149
150 public static Quaternion slerp(Quaternion a, Quaternion b, float interp)
151 {
152 if (dot(a, b) < 0.0)
153 {
154 a.w = -a.w;
155 a.x = -a.x;
156 a.y = -a.y;
157 a.z = -a.z;
158 }
159 float d = dot(a, b);
160 if (d >= 1.0)
161 {
162 return a;
163 }
164 float theta = (float)Math.Acos(d);
165 if (theta == 0.0f)
166 {
167 return (a);
168 }
169 return a * ((float)Math.Sin(theta - interp * theta) / (float)Math.Sin(theta)) + b * ((float)Math.Sin(interp * theta) / (float)Math.Sin(theta));
170 }
171
172 public static Quaternion Interpolate(Quaternion q0, Quaternion q1, float alpha)
173 {
174 return slerp(q0, q1, alpha);
175 }
176
177 public static Quaternion Inverse(Quaternion q)
178 {
179 return new Quaternion(-q.x, -q.y, -q.z, q.w);
180 }
181
182 public static Quaternion YawPitchRoll(float yaw, float pitch, float roll)
183 {
184 roll *= (3.14159264f / 180.0f);
185 yaw *= (3.14159264f / 180.0f);
186 pitch *= (3.14159264f / 180.0f);
187 return new Quaternion(new float3(0.0f, 0.0f, 1.0f), yaw) * new Quaternion(new float3(1.0f, 0.0f, 0.0f), pitch) * new Quaternion(new float3(0.0f, 1.0f, 0.0f), roll);
188 }
189
190 public static float Yaw(Quaternion q)
191 {
192 float3 v = q.ydir();
193 return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f);
194 }
195
196 public static float Pitch(Quaternion q)
197 {
198 float3 v = q.ydir();
199 return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f);
200 }
201
202 public static float Roll(Quaternion q)
203 {
204 q = new Quaternion(new float3(0.0f, 0.0f, 1.0f), -Yaw(q) * (3.14159264f / 180.0f)) * q;
205 q = new Quaternion(new float3(1.0f, 0.0f, 0.0f), -Pitch(q) * (3.14159264f / 180.0f)) * q;
206 return (float)Math.Atan2(-q.xdir().z, q.xdir().x) * (180.0f / 3.14159264f);
207 }
208 }
209}
generated by cgit v1.1 at 2024-12-14 18:39:56 +0000