| 1 |
using System; |
| 2 |
using System.Collections.Generic; |
| 3 |
using Microsoft.Xna.Framework; |
| 4 |
using Freefall.Engine.Support; |
| 5 |
|
| 6 |
namespace Freefall.Engine.Support |
| 7 |
{ |
| 8 |
public struct Rotation |
| 9 |
{ |
| 10 |
private Matrix cMatrix; |
| 11 |
private Quaternion cQuaternion; |
| 12 |
|
| 13 |
private Vector3 up; // = Vector3.Up; |
| 14 |
private Vector3 down; // = Vector3.Down; |
| 15 |
private Vector3 left; // = Vector3.Left; |
| 16 |
private Vector3 right; // = Vector3.Right; |
| 17 |
private Vector3 forward; // = Vector3.Forward; |
| 18 |
private Vector3 backward; // = Vector3.Backward; |
| 19 |
|
| 20 |
#region Fields |
| 21 |
|
| 22 |
private float yaw; |
| 23 |
private float pitch; |
| 24 |
private float roll; |
| 25 |
|
| 26 |
private ClampPair pitchClamp; // = new ClampPair(-180f, 180f, false); |
| 27 |
private ClampPair yawClamp; // = new ClampPair(-180f, 180f, false); |
| 28 |
private ClampPair rollClamp; // = new ClampPair(-180f, 180f, false); |
| 29 |
|
| 30 |
private float x; |
| 31 |
private float y; |
| 32 |
private float z; |
| 33 |
private float w; |
| 34 |
|
| 35 |
public Matrix Matrix |
| 36 |
{ |
| 37 |
get { return CreateMatrix(); } |
| 38 |
} |
| 39 |
|
| 40 |
public Quaternion Quaternion |
| 41 |
{ |
| 42 |
get { return CreateQuaternion(); } |
| 43 |
} |
| 44 |
|
| 45 |
public float Yaw |
| 46 |
{ |
| 47 |
get { return yaw; } |
| 48 |
set { yaw = value; CalcXYZW(); } |
| 49 |
} |
| 50 |
|
| 51 |
public float Pitch |
| 52 |
{ |
| 53 |
get { return pitch; } |
| 54 |
set { pitch = value; CalcXYZW(); } |
| 55 |
} |
| 56 |
|
| 57 |
public float Roll |
| 58 |
{ |
| 59 |
get { return roll; } |
| 60 |
set { roll = value; CalcXYZW(); } |
| 61 |
} |
| 62 |
|
| 63 |
public ClampPair PitchClamp |
| 64 |
{ |
| 65 |
get { return pitchClamp; } |
| 66 |
set { pitchClamp = value; ClampValues(); } |
| 67 |
} |
| 68 |
|
| 69 |
public ClampPair YawClamp |
| 70 |
{ |
| 71 |
get { return yawClamp; } |
| 72 |
set { yawClamp = value; ClampValues(); } |
| 73 |
} |
| 74 |
|
| 75 |
public ClampPair RollClamp |
| 76 |
{ |
| 77 |
get { return rollClamp; } |
| 78 |
set { rollClamp = value; ClampValues(); } |
| 79 |
} |
| 80 |
|
| 81 |
public float X { get { return x; } } |
| 82 |
public float Y { get { return y; } } |
| 83 |
public float Z { get { return z; } } |
| 84 |
public float W { get { return w; } } |
| 85 |
|
| 86 |
#endregion |
| 87 |
|
| 88 |
#region Constructors |
| 89 |
|
| 90 |
public Rotation(float yaw, float pitch, float roll) |
| 91 |
{ |
| 92 |
this.yaw = yaw; |
| 93 |
this.pitch = pitch; |
| 94 |
this.roll = roll; |
| 95 |
|
| 96 |
cQuaternion = Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll); |
| 97 |
up = Vector3.Transform(Vector3.Up, cQuaternion); |
| 98 |
down = Vector3.Transform(Vector3.Down, cQuaternion); |
| 99 |
left = Vector3.Transform(Vector3.Left, cQuaternion); |
| 100 |
right = Vector3.Transform(Vector3.Right, cQuaternion); |
| 101 |
forward = Vector3.Transform(Vector3.Forward, cQuaternion); |
| 102 |
backward = Vector3.Transform(Vector3.Backward, cQuaternion); |
| 103 |
cMatrix = Matrix.CreateFromQuaternion(cQuaternion); |
| 104 |
|
| 105 |
x = cQuaternion.X; |
| 106 |
y = cQuaternion.Y; |
| 107 |
z = cQuaternion.Z; |
| 108 |
w = cQuaternion.W; |
| 109 |
|
| 110 |
pitchClamp = new ClampPair(-180f, 180f, false); |
| 111 |
yawClamp = new ClampPair(-180f, 180f, false); |
| 112 |
rollClamp = new ClampPair(-180f, 180f, false); |
| 113 |
|
| 114 |
CalcXYZW(); |
| 115 |
} |
| 116 |
|
| 117 |
public Rotation(float x, float y, float z, float w) |
| 118 |
{ |
| 119 |
this.x = x; |
| 120 |
this.y = y; |
| 121 |
this.z = z; |
| 122 |
this.w = w; |
| 123 |
|
| 124 |
Rotation xyzw = Rotation.CreateFromQuaternion(new Quaternion(x, y, z, w)); |
| 125 |
this.yaw = xyzw.yaw; |
| 126 |
this.pitch = xyzw.pitch; |
| 127 |
this.roll = xyzw.roll; |
| 128 |
|
| 129 |
up = Vector3.Transform(Vector3.Up, xyzw.Quaternion); |
| 130 |
down = Vector3.Transform(Vector3.Down, xyzw.Quaternion); |
| 131 |
left = Vector3.Transform(Vector3.Left, xyzw.Quaternion); |
| 132 |
right = Vector3.Transform(Vector3.Right, xyzw.Quaternion); |
| 133 |
forward = Vector3.Transform(Vector3.Forward, xyzw.Quaternion); |
| 134 |
backward = Vector3.Transform(Vector3.Backward, xyzw.Quaternion); |
| 135 |
|
| 136 |
pitchClamp = new ClampPair(-180f, 180f, false); |
| 137 |
yawClamp = new ClampPair(-180f, 180f, false); |
| 138 |
rollClamp = new ClampPair(-180f, 180f, false); |
| 139 |
|
| 140 |
cQuaternion = new Quaternion(x, y, z, w); |
| 141 |
cMatrix = Microsoft.Xna.Framework.Matrix.CreateFromQuaternion(cQuaternion); |
| 142 |
} |
| 143 |
|
| 144 |
#endregion |
| 145 |
|
| 146 |
public static Rotation Identity |
| 147 |
{ |
| 148 |
get { return new Rotation(0f, 0f, 0f); } |
| 149 |
} |
| 150 |
|
| 151 |
//public Vector3 Up { get { return Vector3.Transform(Vector3.Up, CreateQuaternion()); } } |
| 152 |
//public Vector3 Down { get { return Vector3.Transform(Vector3.Down, CreateQuaternion()); } } |
| 153 |
//public Vector3 Left { get { return Vector3.Transform(Vector3.Left, CreateQuaternion()); } } |
| 154 |
//public Vector3 Right { get { return Vector3.Transform(Vector3.Right, CreateQuaternion()); } } |
| 155 |
//public Vector3 Forward { get { return Vector3.Transform(Vector3.Forward, CreateQuaternion()); } } |
| 156 |
//public Vector3 Backward { get { return Vector3.Transform(Vector3.Backward, CreateQuaternion()); } } |
| 157 |
|
| 158 |
public Vector3 Up { get { return up; } } |
| 159 |
public Vector3 Down { get { return down; } } |
| 160 |
public Vector3 Left { get { return left; } } |
| 161 |
public Vector3 Right { get { return right; } } |
| 162 |
public Vector3 Forward { get { return forward; } } |
| 163 |
public Vector3 Backward { get { return backward; } } |
| 164 |
|
| 165 |
public Vector3 Transform(Vector3 source) { return Transform(source, this); } |
| 166 |
public static Vector3 Transform(Vector3 source, Rotation rotation) |
| 167 |
{ |
| 168 |
return Vector3.Transform(source, rotation.CreateQuaternion()); |
| 169 |
} |
| 170 |
|
| 171 |
public static Rotation Lerp(Rotation value1, Rotation value2, float amount) |
| 172 |
{ |
| 173 |
Rotation ret = new Rotation(); |
| 174 |
ret.yaw = value1.yaw + ((value2.yaw - value1.yaw) * amount); |
| 175 |
ret.pitch = value1.pitch + ((value2.pitch - value1.pitch) * amount); |
| 176 |
ret.roll = value1.roll + ((value2.roll - value1.roll) * amount); |
| 177 |
ret.CalcXYZW(); |
| 178 |
return ret; |
| 179 |
} |
| 180 |
|
| 181 |
public static Rotation CreateFromLookAt(Vector3 pos, Vector3 look) { return CreateFromLookAt(pos, look, Vector3.Up); } |
| 182 |
public static Rotation CreateFromLookAt(Vector3 pos, Vector3 look, Vector3 up) |
| 183 |
{ |
| 184 |
float pitch = 0f; |
| 185 |
float yaw = 0f; |
| 186 |
float roll = 0f; |
| 187 |
|
| 188 |
pitch = (float)Math.Atan((pos.Y - look.Y) / (pos.X - look.Y)); |
| 189 |
yaw = (float)Math.Atan((pos.Z - look.Z) / (pos.X - look.X)); |
| 190 |
roll = (float)Math.Atan((Vector3.Up.Z - look.Z) / (Vector3.Up.Y - look.Y)); |
| 191 |
|
| 192 |
return new Rotation(yaw, pitch, roll); |
| 193 |
} |
| 194 |
|
| 195 |
public Vector3 CreateEulerAngles() { return CreateEulerAngles(this); } |
| 196 |
public static Vector3 CreateEulerAngles(Rotation rotation) |
| 197 |
{ |
| 198 |
return new Vector3(rotation.yaw, rotation.pitch, rotation.roll); |
| 199 |
} |
| 200 |
|
| 201 |
public Quaternion CreateQuaternion() { return CreateQuaternion(this); } |
| 202 |
public static Quaternion CreateQuaternion(Rotation r) |
| 203 |
{ |
| 204 |
r.CalcXYZW(); |
| 205 |
r.cQuaternion.X = r.X; r.cQuaternion.Y = r.y; r.cQuaternion.Z = r.Z; r.cQuaternion.W = r.w; |
| 206 |
return r.cQuaternion; |
| 207 |
} |
| 208 |
|
| 209 |
public Matrix CreateMatrix() { return CreateMatrix(this); } |
| 210 |
public static Matrix CreateMatrix(Rotation r) |
| 211 |
{ |
| 212 |
float num9 = r.x * r.x; |
| 213 |
float num8 = r.y * r.y; |
| 214 |
float num7 = r.z * r.z; |
| 215 |
float num6 = r.x * r.y; |
| 216 |
float num5 = r.z * r.w; |
| 217 |
float num4 = r.z * r.x; |
| 218 |
float num3 = r.y * r.w; |
| 219 |
float num2 = r.y * r.z; |
| 220 |
float num = r.x * r.w; |
| 221 |
|
| 222 |
r.cMatrix.M11 = 1f - (2f * (num8 + num7)); |
| 223 |
r.cMatrix.M12 = 2f * (num6 + num5); |
| 224 |
r.cMatrix.M13 = 2f * (num4 - num3); |
| 225 |
r.cMatrix.M14 = 0f; |
| 226 |
r.cMatrix.M21 = 2f * (num6 - num5); |
| 227 |
r.cMatrix.M22 = 1f - (2f * (num7 - num9)); |
| 228 |
r.cMatrix.M23 = 2f * (num2 + num3); |
| 229 |
r.cMatrix.M24 = 0f; |
| 230 |
r.cMatrix.M31 = 2f * (num4 + num3); |
| 231 |
r.cMatrix.M32 = 2f * (num2 - num); |
| 232 |
r.cMatrix.M33 = 1f - (2f * (num8 + num9)); |
| 233 |
r.cMatrix.M34 = 0f; |
| 234 |
r.cMatrix.M41 = 0f; |
| 235 |
r.cMatrix.M42 = 0f; |
| 236 |
r.cMatrix.M43 = 0f; |
| 237 |
r.cMatrix.M44 = 1f; |
| 238 |
|
| 239 |
return r.cMatrix; |
| 240 |
} |
| 241 |
|
| 242 |
public static Rotation CreateFromYawPitchRoll(float yaw, float pitch, float roll) |
| 243 |
{ |
| 244 |
Rotation ret = new Rotation(0f, 0f, 0f); |
| 245 |
ret.yaw = yaw; |
| 246 |
ret.pitch = pitch; |
| 247 |
ret.roll = roll; |
| 248 |
ret.ClampValues(); |
| 249 |
return ret; |
| 250 |
} |
| 251 |
|
| 252 |
public static Rotation CreateFromQuaternion(Quaternion q) |
| 253 |
{ |
| 254 |
Rotation ret = new Rotation(0f, 0f, 0f); |
| 255 |
|
| 256 |
ret.yaw = (float)Math.Atan((2f * (q.W * q.X + q.Y * q.Z)) / (1 - 2 * (q.X * q.X + q.Y * q.Y))); |
| 257 |
ret.pitch = (float)Math.Asin(2f * (q.W * q.Y - q.Z * q.X)); |
| 258 |
ret.roll = (float)Math.Atan((2f * (q.W * q.Z + q.X * q.Y)) / (1 - 2 * (q.Y * q.Y + q.Z * q.Z))); |
| 259 |
|
| 260 |
ret.ClampValues(); |
| 261 |
return ret; |
| 262 |
} |
| 263 |
|
| 264 |
#region Operators |
| 265 |
|
| 266 |
public static bool operator ==(Rotation r1, Rotation r2) |
| 267 |
{ |
| 268 |
return ((r1.yaw == r2.yaw) && (r1.pitch == r2.pitch) && (r1.roll == r2.roll)); |
| 269 |
} |
| 270 |
|
| 271 |
public static bool operator !=(Rotation r1, Rotation r2) |
| 272 |
{ |
| 273 |
return !((r1.yaw == r2.yaw) && (r1.pitch == r2.pitch) && (r1.roll == r2.roll)); |
| 274 |
} |
| 275 |
|
| 276 |
public static Rotation operator +(Rotation r1, Rotation r2) |
| 277 |
{ |
| 278 |
Rotation ret = new Rotation(); |
| 279 |
ret.pitch = r1.pitch + r2.pitch; |
| 280 |
ret.yaw = r1.yaw + r2.yaw; |
| 281 |
ret.roll = r1.roll + r2.roll; |
| 282 |
ret.ClampValues(); |
| 283 |
return ret; |
| 284 |
} |
| 285 |
|
| 286 |
public static Rotation operator -(Rotation r1, Rotation r2) |
| 287 |
{ |
| 288 |
Rotation ret = new Rotation(); |
| 289 |
ret.pitch = r1.pitch - r2.pitch; |
| 290 |
ret.yaw = r1.yaw - r2.yaw; |
| 291 |
ret.roll = r1.roll - r2.roll; |
| 292 |
ret.ClampValues(); |
| 293 |
return ret; |
| 294 |
} |
| 295 |
|
| 296 |
public static Rotation operator *(Rotation r1, float amt) |
| 297 |
{ |
| 298 |
Rotation ret = new Rotation(); |
| 299 |
ret.pitch = r1.pitch * amt; |
| 300 |
ret.yaw = r1.yaw * amt; |
| 301 |
ret.roll = r1.roll * amt; |
| 302 |
ret.ClampValues(); |
| 303 |
return ret; |
| 304 |
} |
| 305 |
|
| 306 |
public static Rotation operator /(Rotation r1, float amt) |
| 307 |
{ |
| 308 |
Rotation ret = new Rotation(); |
| 309 |
ret.pitch = r1.pitch / amt; |
| 310 |
ret.yaw = r1.yaw / amt; |
| 311 |
ret.roll = r1.roll / amt; |
| 312 |
ret.ClampValues(); |
| 313 |
return ret; |
| 314 |
} |
| 315 |
|
| 316 |
#endregion |
| 317 |
|
| 318 |
private void CalcXYZW() |
| 319 |
{ |
| 320 |
float num9 = roll * 0.5f; |
| 321 |
float num6 = (float)Math.Sin((double)num9); |
| 322 |
float num5 = (float)Math.Cos((double)num9); |
| 323 |
float num8 = pitch * 0.5f; |
| 324 |
float num4 = (float)Math.Sin((double)num8); |
| 325 |
float num3 = (float)Math.Cos((double)num8); |
| 326 |
float num7 = yaw * 0.5f; |
| 327 |
float num2 = (float)Math.Sin((double)num7); |
| 328 |
float num = (float)Math.Cos((double)num7); |
| 329 |
|
| 330 |
x = ((num * num4) * num5) + ((num2 * num3) * num6); |
| 331 |
y = ((num2 * num3) * num5) - ((num * num4) * num6); |
| 332 |
z = ((num * num3) * num6) - ((num2 * num4) * num5); |
| 333 |
w = ((num * num3) * num5) + ((num2 * num4) * num6); |
| 334 |
|
| 335 |
Quaternion mod = Allocation.Allocate<Quaternion>(new Quaternion(x,y,z,w)); |
| 336 |
//Quaternion mod = new Quaternion(x, y, z, w); |
| 337 |
|
| 338 |
up = Vector3.Transform(Vector3.Up, mod); |
| 339 |
down = Vector3.Transform(Vector3.Down, mod); |
| 340 |
left = Vector3.Transform(Vector3.Left, mod); |
| 341 |
right = Vector3.Transform(Vector3.Right, mod); |
| 342 |
forward = Vector3.Transform(Vector3.Forward, mod); |
| 343 |
backward = Vector3.Transform(Vector3.Backward, mod); |
| 344 |
|
| 345 |
Allocation.Deallocate<Quaternion>(mod); |
| 346 |
} |
| 347 |
|
| 348 |
private void ClampValues() |
| 349 |
{ |
| 350 |
yaw = pitchClamp.Clamp(yaw); |
| 351 |
pitch = pitchClamp.Clamp(pitch); |
| 352 |
roll = pitchClamp.Clamp(roll); |
| 353 |
CalcXYZW(); |
| 354 |
} |
| 355 |
|
| 356 |
public override string ToString() |
| 357 |
{ |
| 358 |
return String.Format("Yaw: {0}, Pitch: {1}, Roll: {2}", yaw, pitch, roll); |
| 359 |
} |
| 360 |
} |
| 361 |
|
| 362 |
public struct ClampPair |
| 363 |
{ |
| 364 |
public bool Active; |
| 365 |
public float Min; |
| 366 |
public float Max; |
| 367 |
public ClampPair(float min, float max) { this.Min = min; this.Max = max; Active = false; } |
| 368 |
public ClampPair(float min, float max, bool active) { this.Min = min; this.Max = max; Active = active; } |
| 369 |
|
| 370 |
public float Clamp(float value) |
| 371 |
{ |
| 372 |
if (Active) |
| 373 |
return MathHelper.Clamp(value, Min, Max); |
| 374 |
else |
| 375 |
return value; |
| 376 |
} |
| 377 |
} |
| 378 |
} |
| 379 |
|