corrade-vassal – Blame information for rev 1
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | vero | 1 | /* |
2 | * Copyright (c) 2006-2014, openmetaverse.org |
||
3 | * All rights reserved. |
||
4 | * |
||
5 | * - Redistribution and use in source and binary forms, with or without |
||
6 | * modification, are permitted provided that the following conditions are met: |
||
7 | * |
||
8 | * - Redistributions of source code must retain the above copyright notice, this |
||
9 | * list of conditions and the following disclaimer. |
||
10 | * - Neither the name of the openmetaverse.org nor the names |
||
11 | * of its contributors may be used to endorse or promote products derived from |
||
12 | * this software without specific prior written permission. |
||
13 | * |
||
14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
||
15 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||
16 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||
17 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
||
18 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||
19 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||
20 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||
21 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
||
22 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||
23 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
24 | * POSSIBILITY OF SUCH DAMAGE. |
||
25 | */ |
||
26 | |||
27 | using System; |
||
28 | |||
29 | namespace OpenMetaverse |
||
30 | { |
||
31 | public class CoordinateFrame |
||
32 | { |
||
33 | public static readonly Vector3 X_AXIS = new Vector3(1f, 0f, 0f); |
||
34 | public static readonly Vector3 Y_AXIS = new Vector3(0f, 1f, 0f); |
||
35 | public static readonly Vector3 Z_AXIS = new Vector3(0f, 0f, 1f); |
||
36 | |||
37 | /// <summary>Origin position of this coordinate frame</summary> |
||
38 | public Vector3 Origin |
||
39 | { |
||
40 | get { return origin; } |
||
41 | set |
||
42 | { |
||
43 | if (!value.IsFinite()) |
||
44 | throw new ArgumentException("Non-finite in CoordinateFrame.Origin assignment"); |
||
45 | origin = value; |
||
46 | } |
||
47 | } |
||
48 | /// <summary>X axis of this coordinate frame, or Forward/At in grid terms</summary> |
||
49 | public Vector3 XAxis |
||
50 | { |
||
51 | get { return xAxis; } |
||
52 | set |
||
53 | { |
||
54 | if (!value.IsFinite()) |
||
55 | throw new ArgumentException("Non-finite in CoordinateFrame.XAxis assignment"); |
||
56 | xAxis = value; |
||
57 | } |
||
58 | } |
||
59 | /// <summary>Y axis of this coordinate frame, or Left in grid terms</summary> |
||
60 | public Vector3 YAxis |
||
61 | { |
||
62 | get { return yAxis; } |
||
63 | set |
||
64 | { |
||
65 | if (!value.IsFinite()) |
||
66 | throw new ArgumentException("Non-finite in CoordinateFrame.YAxis assignment"); |
||
67 | yAxis = value; |
||
68 | } |
||
69 | } |
||
70 | /// <summary>Z axis of this coordinate frame, or Up in grid terms</summary> |
||
71 | public Vector3 ZAxis |
||
72 | { |
||
73 | get { return zAxis; } |
||
74 | set |
||
75 | { |
||
76 | if (!value.IsFinite()) |
||
77 | throw new ArgumentException("Non-finite in CoordinateFrame.ZAxis assignment"); |
||
78 | zAxis = value; |
||
79 | } |
||
80 | } |
||
81 | |||
82 | protected Vector3 origin; |
||
83 | protected Vector3 xAxis; |
||
84 | protected Vector3 yAxis; |
||
85 | protected Vector3 zAxis; |
||
86 | |||
87 | #region Constructors |
||
88 | |||
89 | public CoordinateFrame(Vector3 origin) |
||
90 | { |
||
91 | this.origin = origin; |
||
92 | xAxis = X_AXIS; |
||
93 | yAxis = Y_AXIS; |
||
94 | zAxis = Z_AXIS; |
||
95 | |||
96 | if (!this.origin.IsFinite()) |
||
97 | throw new ArgumentException("Non-finite in CoordinateFrame constructor"); |
||
98 | } |
||
99 | |||
100 | public CoordinateFrame(Vector3 origin, Vector3 direction) |
||
101 | { |
||
102 | this.origin = origin; |
||
103 | LookDirection(direction); |
||
104 | |||
105 | if (!IsFinite()) |
||
106 | throw new ArgumentException("Non-finite in CoordinateFrame constructor"); |
||
107 | } |
||
108 | |||
109 | public CoordinateFrame(Vector3 origin, Vector3 xAxis, Vector3 yAxis, Vector3 zAxis) |
||
110 | { |
||
111 | this.origin = origin; |
||
112 | this.xAxis = xAxis; |
||
113 | this.yAxis = yAxis; |
||
114 | this.zAxis = zAxis; |
||
115 | |||
116 | if (!IsFinite()) |
||
117 | throw new ArgumentException("Non-finite in CoordinateFrame constructor"); |
||
118 | } |
||
119 | |||
120 | public CoordinateFrame(Vector3 origin, Matrix4 rotation) |
||
121 | { |
||
122 | this.origin = origin; |
||
123 | xAxis = rotation.AtAxis; |
||
124 | yAxis = rotation.LeftAxis; |
||
125 | zAxis = rotation.UpAxis; |
||
126 | |||
127 | if (!IsFinite()) |
||
128 | throw new ArgumentException("Non-finite in CoordinateFrame constructor"); |
||
129 | } |
||
130 | |||
131 | public CoordinateFrame(Vector3 origin, Quaternion rotation) |
||
132 | { |
||
133 | Matrix4 m = Matrix4.CreateFromQuaternion(rotation); |
||
134 | |||
135 | this.origin = origin; |
||
136 | xAxis = m.AtAxis; |
||
137 | yAxis = m.LeftAxis; |
||
138 | zAxis = m.UpAxis; |
||
139 | |||
140 | if (!IsFinite()) |
||
141 | throw new ArgumentException("Non-finite in CoordinateFrame constructor"); |
||
142 | } |
||
143 | |||
144 | #endregion Constructors |
||
145 | |||
146 | #region Public Methods |
||
147 | |||
148 | public void ResetAxes() |
||
149 | { |
||
150 | xAxis = X_AXIS; |
||
151 | yAxis = Y_AXIS; |
||
152 | zAxis = Z_AXIS; |
||
153 | } |
||
154 | |||
155 | public void Rotate(float angle, Vector3 rotationAxis) |
||
156 | { |
||
157 | Quaternion q = Quaternion.CreateFromAxisAngle(rotationAxis, angle); |
||
158 | Rotate(q); |
||
159 | } |
||
160 | |||
161 | public void Rotate(Quaternion q) |
||
162 | { |
||
163 | Matrix4 m = Matrix4.CreateFromQuaternion(q); |
||
164 | Rotate(m); |
||
165 | } |
||
166 | |||
167 | public void Rotate(Matrix4 m) |
||
168 | { |
||
169 | xAxis = Vector3.Transform(xAxis, m); |
||
170 | yAxis = Vector3.Transform(yAxis, m); |
||
171 | |||
172 | Orthonormalize(); |
||
173 | |||
174 | if (!IsFinite()) |
||
175 | throw new Exception("Non-finite in CoordinateFrame.Rotate()"); |
||
176 | } |
||
177 | |||
178 | public void Roll(float angle) |
||
179 | { |
||
180 | Quaternion q = Quaternion.CreateFromAxisAngle(xAxis, angle); |
||
181 | Matrix4 m = Matrix4.CreateFromQuaternion(q); |
||
182 | Rotate(m); |
||
183 | |||
184 | if (!yAxis.IsFinite() || !zAxis.IsFinite()) |
||
185 | throw new Exception("Non-finite in CoordinateFrame.Roll()"); |
||
186 | } |
||
187 | |||
188 | public void Pitch(float angle) |
||
189 | { |
||
190 | Quaternion q = Quaternion.CreateFromAxisAngle(yAxis, angle); |
||
191 | Matrix4 m = Matrix4.CreateFromQuaternion(q); |
||
192 | Rotate(m); |
||
193 | |||
194 | if (!xAxis.IsFinite() || !zAxis.IsFinite()) |
||
195 | throw new Exception("Non-finite in CoordinateFrame.Pitch()"); |
||
196 | } |
||
197 | |||
198 | public void Yaw(float angle) |
||
199 | { |
||
200 | Quaternion q = Quaternion.CreateFromAxisAngle(zAxis, angle); |
||
201 | Matrix4 m = Matrix4.CreateFromQuaternion(q); |
||
202 | Rotate(m); |
||
203 | |||
204 | if (!xAxis.IsFinite() || !yAxis.IsFinite()) |
||
205 | throw new Exception("Non-finite in CoordinateFrame.Yaw()"); |
||
206 | } |
||
207 | |||
208 | public void LookDirection(Vector3 at) |
||
209 | { |
||
210 | LookDirection(at, Z_AXIS); |
||
211 | } |
||
212 | |||
213 | /// <summary> |
||
214 | /// |
||
215 | /// </summary> |
||
216 | /// <param name="at">Looking direction, must be a normalized vector</param> |
||
217 | /// <param name="upDirection">Up direction, must be a normalized vector</param> |
||
218 | public void LookDirection(Vector3 at, Vector3 upDirection) |
||
219 | { |
||
220 | // The two parameters cannot be parallel |
||
221 | Vector3 left = Vector3.Cross(upDirection, at); |
||
222 | if (left == Vector3.Zero) |
||
223 | { |
||
224 | // Prevent left from being zero |
||
225 | at.X += 0.01f; |
||
226 | at.Normalize(); |
||
227 | left = Vector3.Cross(upDirection, at); |
||
228 | } |
||
229 | left.Normalize(); |
||
230 | |||
231 | xAxis = at; |
||
232 | yAxis = left; |
||
233 | zAxis = Vector3.Cross(at, left); |
||
234 | } |
||
235 | |||
236 | /// <summary> |
||
237 | /// Align the coordinate frame X and Y axis with a given rotation |
||
238 | /// around the Z axis in radians |
||
239 | /// </summary> |
||
240 | /// <param name="heading">Absolute rotation around the Z axis in |
||
241 | /// radians</param> |
||
242 | public void LookDirection(double heading) |
||
243 | { |
||
244 | yAxis.X = (float)Math.Cos(heading); |
||
245 | yAxis.Y = (float)Math.Sin(heading); |
||
246 | xAxis.X = (float)-Math.Sin(heading); |
||
247 | xAxis.Y = (float)Math.Cos(heading); |
||
248 | } |
||
249 | |||
250 | public void LookAt(Vector3 origin, Vector3 target) |
||
251 | { |
||
252 | LookAt(origin, target, new Vector3(0f, 0f, 1f)); |
||
253 | } |
||
254 | |||
255 | public void LookAt(Vector3 origin, Vector3 target, Vector3 upDirection) |
||
256 | { |
||
257 | this.origin = origin; |
||
258 | Vector3 at = new Vector3(target - origin); |
||
259 | at.Normalize(); |
||
260 | |||
261 | LookDirection(at, upDirection); |
||
262 | } |
||
263 | |||
264 | #endregion Public Methods |
||
265 | |||
266 | protected bool IsFinite() |
||
267 | { |
||
268 | if (xAxis.IsFinite() && yAxis.IsFinite() && zAxis.IsFinite()) |
||
269 | return true; |
||
270 | else |
||
271 | return false; |
||
272 | } |
||
273 | |||
274 | protected void Orthonormalize() |
||
275 | { |
||
276 | // Make sure the axis are orthagonal and normalized |
||
277 | xAxis.Normalize(); |
||
278 | yAxis -= xAxis * (xAxis * yAxis); |
||
279 | yAxis.Normalize(); |
||
280 | zAxis = Vector3.Cross(xAxis, yAxis); |
||
281 | } |
||
282 | } |
||
283 | } |