corrade-vassal – Blame information for rev 1

Subversion Repositories:
Rev:
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 }