opensim-development – Blame information for rev 1
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | eva | 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 | |||
28 | using System; |
||
29 | |||
30 | namespace 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 | } |