| 1 |
using System; |
| 2 |
|
| 3 |
namespace Oni |
| 4 |
{ |
| 5 |
internal struct Matrix : IEquatable<Matrix> |
| 6 |
{ |
| 7 |
public float M11, M12, M13, M14; |
| 8 |
public float M21, M22, M23, M24; |
| 9 |
public float M31, M32, M33, M34; |
| 10 |
public float M41, M42, M43, M44; |
| 11 |
|
| 12 |
public Matrix(float m11, float m12, float m13, float m14, |
| 13 |
float m21, float m22, float m23, float m24, |
| 14 |
float m31, float m32, float m33, float m34, |
| 15 |
float m41, float m42, float m43, float m44) |
| 16 |
{ |
| 17 |
M11 = m11; M12 = m12; M13 = m13; M14 = m14; |
| 18 |
M21 = m21; M22 = m22; M23 = m23; M24 = m24; |
| 19 |
M31 = m31; M32 = m32; M33 = m33; M34 = m34; |
| 20 |
M41 = m41; M42 = m42; M43 = m43; M44 = m44; |
| 21 |
} |
| 22 |
|
| 23 |
public Matrix(float[] values) |
| 24 |
{ |
| 25 |
M11 = values[0]; M12 = values[4]; M13 = values[8]; M14 = values[12]; |
| 26 |
M21 = values[1]; M22 = values[5]; M23 = values[9]; M24 = values[13]; |
| 27 |
M31 = values[2]; M32 = values[6]; M33 = values[10]; M34 = values[14]; |
| 28 |
M41 = values[3]; M42 = values[7]; M43 = values[11]; M44 = values[15]; |
| 29 |
} |
| 30 |
|
| 31 |
public void CopyTo(float[] values) |
| 32 |
{ |
| 33 |
values[0] = M11; |
| 34 |
values[1] = M21; |
| 35 |
values[2] = M31; |
| 36 |
values[3] = M41; |
| 37 |
|
| 38 |
values[4] = M12; |
| 39 |
values[5] = M22; |
| 40 |
values[6] = M32; |
| 41 |
values[7] = M42; |
| 42 |
|
| 43 |
values[8] = M13; |
| 44 |
values[9] = M23; |
| 45 |
values[10] = M33; |
| 46 |
values[11] = M43; |
| 47 |
|
| 48 |
values[12] = M14; |
| 49 |
values[13] = M24; |
| 50 |
values[14] = M34; |
| 51 |
values[15] = M44; |
| 52 |
} |
| 53 |
|
| 54 |
public static Matrix CreateTranslation(float x, float y, float z) |
| 55 |
{ |
| 56 |
Matrix r = Identity; |
| 57 |
|
| 58 |
r.M41 = x; |
| 59 |
r.M42 = y; |
| 60 |
r.M43 = z; |
| 61 |
|
| 62 |
return r; |
| 63 |
} |
| 64 |
|
| 65 |
public static Matrix CreateTranslation(Vector3 v) => CreateTranslation(v.X, v.Y, v.Z); |
| 66 |
|
| 67 |
public static Matrix CreateScale(float sx, float sy, float sz) |
| 68 |
{ |
| 69 |
Matrix r = Identity; |
| 70 |
|
| 71 |
r.M11 = sx; |
| 72 |
r.M22 = sy; |
| 73 |
r.M33 = sz; |
| 74 |
|
| 75 |
return r; |
| 76 |
} |
| 77 |
|
| 78 |
public static Matrix CreateScale(float s) => CreateScale(s, s, s); |
| 79 |
public static Matrix CreateScale(Vector3 s) => CreateScale(s.X, s.Y, s.Z); |
| 80 |
|
| 81 |
public static Matrix CreateRotationX(float angle) |
| 82 |
{ |
| 83 |
float cos = FMath.Cos(angle); |
| 84 |
float sin = FMath.Sin(angle); |
| 85 |
|
| 86 |
Matrix r = Identity; |
| 87 |
r.M22 = cos; |
| 88 |
r.M23 = sin; |
| 89 |
r.M32 = -sin; |
| 90 |
r.M33 = cos; |
| 91 |
return r; |
| 92 |
} |
| 93 |
|
| 94 |
public static Matrix CreateRotationY(float angle) |
| 95 |
{ |
| 96 |
float cos = FMath.Cos(angle); |
| 97 |
float sin = FMath.Sin(angle); |
| 98 |
|
| 99 |
Matrix r = Identity; |
| 100 |
r.M11 = cos; |
| 101 |
r.M13 = -sin; |
| 102 |
r.M31 = sin; |
| 103 |
r.M33 = cos; |
| 104 |
return r; |
| 105 |
} |
| 106 |
|
| 107 |
public static Matrix CreateRotationZ(float angle) |
| 108 |
{ |
| 109 |
float cos = FMath.Cos(angle); |
| 110 |
float sin = FMath.Sin(angle); |
| 111 |
|
| 112 |
Matrix r = Identity; |
| 113 |
r.M11 = cos; |
| 114 |
r.M12 = sin; |
| 115 |
r.M21 = -sin; |
| 116 |
r.M22 = cos; |
| 117 |
return r; |
| 118 |
} |
| 119 |
|
| 120 |
public static Matrix CreateFromAxisAngle(Vector3 axis, float angle) |
| 121 |
{ |
| 122 |
float sin = FMath.Sin(angle); |
| 123 |
float cos = FMath.Cos(angle); |
| 124 |
|
| 125 |
float x = axis.X; |
| 126 |
float y = axis.Y; |
| 127 |
float z = axis.Z; |
| 128 |
float xx = x * x; |
| 129 |
float yy = y * y; |
| 130 |
float zz = z * z; |
| 131 |
float xy = x * y; |
| 132 |
float xz = x * z; |
| 133 |
float yz = y * z; |
| 134 |
|
| 135 |
Matrix r = Identity; |
| 136 |
r.M11 = xx + (cos * (1.0f - xx)); |
| 137 |
r.M12 = (xy - (cos * xy)) + (sin * z); |
| 138 |
r.M13 = (xz - (cos * xz)) - (sin * y); |
| 139 |
r.M21 = (xy - (cos * xy)) - (sin * z); |
| 140 |
r.M22 = yy + (cos * (1.0f - yy)); |
| 141 |
r.M23 = (yz - (cos * yz)) + (sin * x); |
| 142 |
r.M31 = (xz - (cos * xz)) + (sin * y); |
| 143 |
r.M32 = (yz - (cos * yz)) - (sin * x); |
| 144 |
r.M33 = zz + (cos * (1.0f - zz)); |
| 145 |
return r; |
| 146 |
} |
| 147 |
|
| 148 |
public static Matrix CreateFromQuaternion(Quaternion q) |
| 149 |
{ |
| 150 |
float xx = q.X * q.X; |
| 151 |
float yy = q.Y * q.Y; |
| 152 |
float zz = q.Z * q.Z; |
| 153 |
float xy = q.X * q.Y; |
| 154 |
float zw = q.Z * q.W; |
| 155 |
float zx = q.Z * q.X; |
| 156 |
float yw = q.Y * q.W; |
| 157 |
float yz = q.Y * q.Z; |
| 158 |
float xw = q.X * q.W; |
| 159 |
|
| 160 |
Matrix r = Identity; |
| 161 |
|
| 162 |
r.M11 = 1.0f - 2.0f * (yy + zz); |
| 163 |
r.M12 = 2.0f * (xy + zw); |
| 164 |
r.M13 = 2.0f * (zx - yw); |
| 165 |
|
| 166 |
r.M21 = 2.0f * (xy - zw); |
| 167 |
r.M22 = 1.0f - 2.0f * (zz + xx); |
| 168 |
r.M23 = 2.0f * (yz + xw); |
| 169 |
|
| 170 |
r.M31 = 2.0f * (zx + yw); |
| 171 |
r.M32 = 2.0f * (yz - xw); |
| 172 |
r.M33 = 1.0f - 2.0f * (yy + xx); |
| 173 |
|
| 174 |
return r; |
| 175 |
} |
| 176 |
|
| 177 |
public static Matrix operator +(Matrix m1, Matrix m2) |
| 178 |
{ |
| 179 |
m1.M11 += m2.M11; |
| 180 |
m1.M12 += m2.M12; |
| 181 |
m1.M13 += m2.M13; |
| 182 |
m1.M14 += m2.M14; |
| 183 |
m1.M21 += m2.M21; |
| 184 |
m1.M22 += m2.M22; |
| 185 |
m1.M23 += m2.M23; |
| 186 |
m1.M24 += m2.M24; |
| 187 |
m1.M31 += m2.M31; |
| 188 |
m1.M32 += m2.M32; |
| 189 |
m1.M33 += m2.M33; |
| 190 |
m1.M34 += m2.M34; |
| 191 |
m1.M41 += m2.M41; |
| 192 |
m1.M42 += m2.M42; |
| 193 |
m1.M43 += m2.M43; |
| 194 |
m1.M44 += m2.M44; |
| 195 |
|
| 196 |
return m1; |
| 197 |
} |
| 198 |
|
| 199 |
public static Matrix operator -(Matrix m1, Matrix m2) |
| 200 |
{ |
| 201 |
m1.M11 -= m2.M11; |
| 202 |
m1.M12 -= m2.M12; |
| 203 |
m1.M13 -= m2.M13; |
| 204 |
m1.M14 -= m2.M14; |
| 205 |
m1.M21 -= m2.M21; |
| 206 |
m1.M22 -= m2.M22; |
| 207 |
m1.M23 -= m2.M23; |
| 208 |
m1.M24 -= m2.M24; |
| 209 |
m1.M31 -= m2.M31; |
| 210 |
m1.M32 -= m2.M32; |
| 211 |
m1.M33 -= m2.M33; |
| 212 |
m1.M34 -= m2.M34; |
| 213 |
m1.M41 -= m2.M41; |
| 214 |
m1.M42 -= m2.M42; |
| 215 |
m1.M43 -= m2.M43; |
| 216 |
m1.M44 -= m2.M44; |
| 217 |
|
| 218 |
return m1; |
| 219 |
} |
| 220 |
|
| 221 |
public static Matrix operator *(Matrix m, float s) |
| 222 |
{ |
| 223 |
m.M11 *= s; |
| 224 |
m.M12 *= s; |
| 225 |
m.M13 *= s; |
| 226 |
m.M14 *= s; |
| 227 |
m.M21 *= s; |
| 228 |
m.M22 *= s; |
| 229 |
m.M23 *= s; |
| 230 |
m.M24 *= s; |
| 231 |
m.M31 *= s; |
| 232 |
m.M32 *= s; |
| 233 |
m.M33 *= s; |
| 234 |
m.M34 *= s; |
| 235 |
m.M41 *= s; |
| 236 |
m.M42 *= s; |
| 237 |
m.M43 *= s; |
| 238 |
m.M44 *= s; |
| 239 |
|
| 240 |
return m; |
| 241 |
} |
| 242 |
|
| 243 |
public static Matrix operator *(float s, Matrix m) => m * s; |
| 244 |
|
| 245 |
public static Matrix operator /(Matrix m, float s) => m * (1.0f / s); |
| 246 |
|
| 247 |
public static Matrix operator *(Matrix m1, Matrix m2) |
| 248 |
{ |
| 249 |
Matrix r; |
| 250 |
|
| 251 |
r.M11 = m1.M11 * m2.M11 + m1.M12 * m2.M21 + m1.M13 * m2.M31 + m1.M14 * m2.M41; |
| 252 |
r.M12 = m1.M11 * m2.M12 + m1.M12 * m2.M22 + m1.M13 * m2.M32 + m1.M14 * m2.M42; |
| 253 |
r.M13 = m1.M11 * m2.M13 + m1.M12 * m2.M23 + m1.M13 * m2.M33 + m1.M14 * m2.M43; |
| 254 |
r.M14 = m1.M11 * m2.M14 + m1.M12 * m2.M24 + m1.M13 * m2.M34 + m1.M14 * m2.M44; |
| 255 |
r.M21 = m1.M21 * m2.M11 + m1.M22 * m2.M21 + m1.M23 * m2.M31 + m1.M24 * m2.M41; |
| 256 |
r.M22 = m1.M21 * m2.M12 + m1.M22 * m2.M22 + m1.M23 * m2.M32 + m1.M24 * m2.M42; |
| 257 |
r.M23 = m1.M21 * m2.M13 + m1.M22 * m2.M23 + m1.M23 * m2.M33 + m1.M24 * m2.M43; |
| 258 |
r.M24 = m1.M21 * m2.M14 + m1.M22 * m2.M24 + m1.M23 * m2.M34 + m1.M24 * m2.M44; |
| 259 |
r.M31 = m1.M31 * m2.M11 + m1.M32 * m2.M21 + m1.M33 * m2.M31 + m1.M34 * m2.M41; |
| 260 |
r.M32 = m1.M31 * m2.M12 + m1.M32 * m2.M22 + m1.M33 * m2.M32 + m1.M34 * m2.M42; |
| 261 |
r.M33 = m1.M31 * m2.M13 + m1.M32 * m2.M23 + m1.M33 * m2.M33 + m1.M34 * m2.M43; |
| 262 |
r.M34 = m1.M31 * m2.M14 + m1.M32 * m2.M24 + m1.M33 * m2.M34 + m1.M34 * m2.M44; |
| 263 |
r.M41 = m1.M41 * m2.M11 + m1.M42 * m2.M21 + m1.M43 * m2.M31 + m1.M44 * m2.M41; |
| 264 |
r.M42 = m1.M41 * m2.M12 + m1.M42 * m2.M22 + m1.M43 * m2.M32 + m1.M44 * m2.M42; |
| 265 |
r.M43 = m1.M41 * m2.M13 + m1.M42 * m2.M23 + m1.M43 * m2.M33 + m1.M44 * m2.M43; |
| 266 |
r.M44 = m1.M41 * m2.M14 + m1.M42 * m2.M24 + m1.M43 * m2.M34 + m1.M44 * m2.M44; |
| 267 |
|
| 268 |
return r; |
| 269 |
} |
| 270 |
|
| 271 |
public Matrix Transpose() |
| 272 |
{ |
| 273 |
Matrix t; |
| 274 |
|
| 275 |
t.M11 = M11; |
| 276 |
t.M12 = M21; |
| 277 |
t.M13 = M31; |
| 278 |
t.M14 = M41; |
| 279 |
t.M21 = M12; |
| 280 |
t.M22 = M22; |
| 281 |
t.M23 = M32; |
| 282 |
t.M24 = M42; |
| 283 |
t.M31 = M13; |
| 284 |
t.M32 = M23; |
| 285 |
t.M33 = M33; |
| 286 |
t.M34 = M43; |
| 287 |
t.M41 = M14; |
| 288 |
t.M42 = M24; |
| 289 |
t.M43 = M34; |
| 290 |
t.M44 = M44; |
| 291 |
|
| 292 |
return t; |
| 293 |
} |
| 294 |
|
| 295 |
public static bool operator ==(Matrix m1, Matrix m2) => m1.Equals(m2); |
| 296 |
public static bool operator !=(Matrix m1, Matrix m2) => !m1.Equals(m2); |
| 297 |
|
| 298 |
public Vector3 XAxis |
| 299 |
{ |
| 300 |
get |
| 301 |
{ |
| 302 |
return new Vector3(M11, M12, M13); |
| 303 |
} |
| 304 |
set |
| 305 |
{ |
| 306 |
M11 = value.X; |
| 307 |
M12 = value.Y; |
| 308 |
M13 = value.Z; |
| 309 |
} |
| 310 |
} |
| 311 |
|
| 312 |
public Vector3 YAxis |
| 313 |
{ |
| 314 |
get |
| 315 |
{ |
| 316 |
return new Vector3(M21, M22, M23); |
| 317 |
} |
| 318 |
set |
| 319 |
{ |
| 320 |
M21 = value.X; |
| 321 |
M22 = value.Y; |
| 322 |
M23 = value.Z; |
| 323 |
} |
| 324 |
} |
| 325 |
|
| 326 |
public Vector3 ZAxis |
| 327 |
{ |
| 328 |
get |
| 329 |
{ |
| 330 |
return new Vector3(M31, M32, M33); |
| 331 |
} |
| 332 |
set |
| 333 |
{ |
| 334 |
M31 = value.X; |
| 335 |
M32 = value.Y; |
| 336 |
M33 = value.Z; |
| 337 |
} |
| 338 |
} |
| 339 |
|
| 340 |
public Vector3 Scale |
| 341 |
{ |
| 342 |
get |
| 343 |
{ |
| 344 |
return new Vector3(M11, M22, M33); |
| 345 |
} |
| 346 |
set |
| 347 |
{ |
| 348 |
M11 = value.X; |
| 349 |
M22 = value.Y; |
| 350 |
M33 = value.Z; |
| 351 |
} |
| 352 |
} |
| 353 |
|
| 354 |
public Vector3 Translation |
| 355 |
{ |
| 356 |
get |
| 357 |
{ |
| 358 |
return new Vector3(M41, M42, M43); |
| 359 |
} |
| 360 |
set |
| 361 |
{ |
| 362 |
M41 = value.X; |
| 363 |
M42 = value.Y; |
| 364 |
M43 = value.Z; |
| 365 |
} |
| 366 |
} |
| 367 |
|
| 368 |
public bool Equals(Matrix other) |
| 369 |
{ |
| 370 |
return (M11 == other.M11 && M12 == other.M12 && M13 == other.M13 && M14 == other.M14 |
| 371 |
&& M21 == other.M21 && M22 == other.M22 && M23 == other.M23 && M24 == other.M24 |
| 372 |
&& M31 == other.M31 && M32 == other.M32 && M33 == other.M33 && M34 == other.M34 |
| 373 |
&& M41 == other.M41 && M42 == other.M42 && M43 == other.M43 && M44 == other.M44); |
| 374 |
} |
| 375 |
|
| 376 |
public override bool Equals(object obj) => obj is Matrix && Equals((Matrix)obj); |
| 377 |
|
| 378 |
public override int GetHashCode() |
| 379 |
{ |
| 380 |
return M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode() |
| 381 |
^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode() |
| 382 |
^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode() |
| 383 |
^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode(); |
| 384 |
} |
| 385 |
|
| 386 |
public override string ToString() |
| 387 |
{ |
| 388 |
return string.Format("{{M11:{0} M12:{1} M13:{2} M14:{3}}}\n{{M21:{4} M22:{5} M23:{6} M24:{7}}}\n{{M31:{8} M32:{9} M33:{10} M34:{11}}}\n{{M41:{12} M42:{13} M43:{14} M44:{15}}}", |
| 389 |
M11, M12, M13, M14, |
| 390 |
M21, M22, M23, M24, |
| 391 |
M31, M32, M33, M34, |
| 392 |
M41, M42, M43, M44); |
| 393 |
} |
| 394 |
|
| 395 |
private static readonly Matrix identity = new Matrix( |
| 396 |
1.0f, 0.0f, 0.0f, 0.0f, |
| 397 |
0.0f, 1.0f, 0.0f, 0.0f, |
| 398 |
0.0f, 0.0f, 1.0f, 0.0f, |
| 399 |
0.0f, 0.0f, 0.0f, 1.0f); |
| 400 |
|
| 401 |
public static Matrix Identity => identity; |
| 402 |
|
| 403 |
public Vector3 ToEuler() |
| 404 |
{ |
| 405 |
float a = M11; |
| 406 |
float b = M21; |
| 407 |
float c, s, r; |
| 408 |
|
| 409 |
if (b == 0.0f) |
| 410 |
{ |
| 411 |
c = FMath.Sign(a); |
| 412 |
s = 0.0f; |
| 413 |
r = Math.Abs(a); |
| 414 |
} |
| 415 |
else if (a == 0.0f) |
| 416 |
{ |
| 417 |
c = 0.0f; |
| 418 |
s = FMath.Sign(b); |
| 419 |
r = Math.Abs(b); |
| 420 |
} |
| 421 |
else if (Math.Abs(b) > Math.Abs(a)) |
| 422 |
{ |
| 423 |
float t = a / b; |
| 424 |
float u = FMath.Sign(b) * FMath.Sqrt(1.0f + t * t); |
| 425 |
s = 1.0f / u; |
| 426 |
c = s * t; |
| 427 |
r = b * u; |
| 428 |
} |
| 429 |
else |
| 430 |
{ |
| 431 |
float t = b / a; |
| 432 |
float u = FMath.Sign(a) * FMath.Sqrt(1.0f + t * t); |
| 433 |
c = 1.0f / u; |
| 434 |
s = c * t; |
| 435 |
r = a * u; |
| 436 |
} |
| 437 |
|
| 438 |
Vector3 e; |
| 439 |
e.Z = MathHelper.ToDegrees(-FMath.Atan2(s, c)); |
| 440 |
e.Y = MathHelper.ToDegrees(FMath.Atan2(M31, r)); |
| 441 |
e.X = MathHelper.ToDegrees(-FMath.Atan2(M32, M33)); |
| 442 |
return e; |
| 443 |
} |
| 444 |
|
| 445 |
public float Determinant() |
| 446 |
{ |
| 447 |
var m11 = M11; |
| 448 |
var m12 = M12; |
| 449 |
var m13 = M13; |
| 450 |
var m14 = M14; |
| 451 |
var m21 = M21; |
| 452 |
var m22 = M22; |
| 453 |
var m23 = M23; |
| 454 |
var m24 = M24; |
| 455 |
var m31 = M31; |
| 456 |
var m32 = M32; |
| 457 |
var m33 = M33; |
| 458 |
var m34 = M34; |
| 459 |
var m41 = M41; |
| 460 |
var m42 = M42; |
| 461 |
var m43 = M43; |
| 462 |
var m44 = M44; |
| 463 |
|
| 464 |
var d3434 = m33 * m44 - m34 * m43; |
| 465 |
var d3424 = m32 * m44 - m34 * m42; |
| 466 |
var d3423 = m32 * m43 - m33 * m42; |
| 467 |
var d3414 = m31 * m44 - m34 * m41; |
| 468 |
var d3413 = m31 * m43 - m33 * m41; |
| 469 |
var d3412 = m31 * m42 - m32 * m41; |
| 470 |
|
| 471 |
return m11 * (m22 * d3434 - m23 * d3424 + m24 * d3423) |
| 472 |
- m12 * (m21 * d3434 - m23 * d3414 + m24 * d3413) |
| 473 |
+ m13 * (m21 * d3424 - m22 * d3414 + m24 * d3412) |
| 474 |
- m14 * (m21 * d3423 - m22 * d3413 + m23 * d3412); |
| 475 |
} |
| 476 |
|
| 477 |
//Matrix m = Matrix.Identity; |
| 478 |
//m *= Matrix.CreateScale(3.3f, 1.3f, 7.6f); |
| 479 |
//m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f); |
| 480 |
//m *= Matrix.CreateRotationY(1.2f); |
| 481 |
//m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f); |
| 482 |
//m *= Matrix.CreateRotationY(1.2f); |
| 483 |
//m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f); |
| 484 |
//m *= Matrix.CreateRotationY(1.2f); |
| 485 |
|
| 486 |
//Vector3 s, t; |
| 487 |
//Quaternion r; |
| 488 |
//m.Decompose(out s, out r, out t); |
| 489 |
//Matrix m2 = Matrix.CreateScale(s) * Matrix.CreateFromQuaternion(r) * Matrix.CreateTranslation(t); |
| 490 |
|
| 491 |
//Console.WriteLine(m2 - m); |
| 492 |
//return 0; |
| 493 |
|
| 494 |
//[StructLayout(LayoutKind.Sequential)] |
| 495 |
//private unsafe struct VectorBasis |
| 496 |
//{ |
| 497 |
// public Vector3* axis0; |
| 498 |
// public Vector3* axis1; |
| 499 |
// public Vector3* axis2; |
| 500 |
//} |
| 501 |
|
| 502 |
//[StructLayout(LayoutKind.Sequential)] |
| 503 |
//private struct CanonicalBasis |
| 504 |
//{ |
| 505 |
// public Vector3 axis0; |
| 506 |
// public Vector3 axis1; |
| 507 |
// public Vector3 axis2; |
| 508 |
//} |
| 509 |
|
| 510 |
//public unsafe bool Decompose(out Vector3 outScale, out Quaternion outRotation, out Vector3 outTranslation) |
| 511 |
//{ |
| 512 |
// outTranslation.X = M41; |
| 513 |
// outTranslation.Y = M42; |
| 514 |
// outTranslation.Z = M43; |
| 515 |
|
| 516 |
// var rotation = new Matrix( |
| 517 |
// M11, M12, M13, 0.0f, |
| 518 |
// M21, M22, M23, 0.0f, |
| 519 |
// M31, M32, M33, 0.0f, |
| 520 |
// 0.0f, 0.0f, 0.0f, 1.0f); |
| 521 |
|
| 522 |
// var vectorBasis = new VectorBasis { |
| 523 |
// axis0 = (Vector3*)&rotation.M11, |
| 524 |
// axis1 = (Vector3*)&rotation.M21, |
| 525 |
// axis2 = (Vector3*)&rotation.M31 |
| 526 |
// }; |
| 527 |
|
| 528 |
// var canonicalBasis = new CanonicalBasis { |
| 529 |
// axis0 = Vector3.UnitX, |
| 530 |
// axis1 = Vector3.UnitY, |
| 531 |
// axis2 = Vector3.UnitZ |
| 532 |
// }; |
| 533 |
|
| 534 |
// var scale = new Vector3( |
| 535 |
// vectorBasis.axis0->Length(), |
| 536 |
// vectorBasis.axis1->Length(), |
| 537 |
// vectorBasis.axis2->Length() |
| 538 |
// ); |
| 539 |
|
| 540 |
// int xi, yi, zi; |
| 541 |
|
| 542 |
// if (scale.X < scale.Y) |
| 543 |
// { |
| 544 |
// if (scale.Y < scale.Z) |
| 545 |
// { |
| 546 |
// xi = 2; |
| 547 |
// yi = 1; |
| 548 |
// zi = 0; |
| 549 |
// } |
| 550 |
// else |
| 551 |
// { |
| 552 |
// xi = 1; |
| 553 |
|
| 554 |
// if (scale.X < scale.Z) |
| 555 |
// { |
| 556 |
// yi = 2; |
| 557 |
// zi = 0; |
| 558 |
// } |
| 559 |
// else |
| 560 |
// { |
| 561 |
// yi = 0; |
| 562 |
// zi = 2; |
| 563 |
// } |
| 564 |
// } |
| 565 |
// } |
| 566 |
// else |
| 567 |
// { |
| 568 |
// if (scale.X < scale.Z) |
| 569 |
// { |
| 570 |
// xi = 2; |
| 571 |
// yi = 0; |
| 572 |
// zi = 1; |
| 573 |
// } |
| 574 |
// else |
| 575 |
// { |
| 576 |
// xi = 0; |
| 577 |
|
| 578 |
// if (scale.Y < scale.Z) |
| 579 |
// { |
| 580 |
// yi = 2; |
| 581 |
// zi = 1; |
| 582 |
// } |
| 583 |
// else |
| 584 |
// { |
| 585 |
// yi = 1; |
| 586 |
// zi = 2; |
| 587 |
// } |
| 588 |
// } |
| 589 |
// } |
| 590 |
|
| 591 |
// var pScale = &scale.X; |
| 592 |
|
| 593 |
// var pvBasis = &vectorBasis.axis0; |
| 594 |
// var pcBasis = &canonicalBasis.axis0; |
| 595 |
|
| 596 |
// if (pScale[xi] < 0.0001f) |
| 597 |
// { |
| 598 |
// // |
| 599 |
// // If the smallest scale is < 0.0001 then use the coresponding cannonical basis instead |
| 600 |
// // |
| 601 |
|
| 602 |
// pvBasis[xi] = &pcBasis[xi]; |
| 603 |
// } |
| 604 |
// else |
| 605 |
// { |
| 606 |
// pvBasis[xi]->Normalize(); |
| 607 |
// } |
| 608 |
|
| 609 |
// if (pScale[yi] < 0.0001f) |
| 610 |
// { |
| 611 |
// // |
| 612 |
// // The second smallest scale is < 0.0001 too, build a perpendicular vector |
| 613 |
// // |
| 614 |
|
| 615 |
// float fx = Math.Abs(pvBasis[xi]->X); |
| 616 |
// float fy = Math.Abs(pvBasis[xi]->Y); |
| 617 |
// float fz = Math.Abs(pvBasis[xi]->Z); |
| 618 |
|
| 619 |
// int yij; |
| 620 |
|
| 621 |
// if (fx < fy) |
| 622 |
// { |
| 623 |
// if (fy < fz) |
| 624 |
// { |
| 625 |
// yij = 0; |
| 626 |
// } |
| 627 |
// else |
| 628 |
// { |
| 629 |
// if (fx < fz) |
| 630 |
// yij = 0; |
| 631 |
// else |
| 632 |
// yij = 2; |
| 633 |
// } |
| 634 |
// } |
| 635 |
// else |
| 636 |
// { |
| 637 |
// if (fx < fz) |
| 638 |
// { |
| 639 |
// yij = 1; |
| 640 |
// } |
| 641 |
// else |
| 642 |
// { |
| 643 |
// if (fy < fz) |
| 644 |
// yij = 1; |
| 645 |
// else |
| 646 |
// yij = 2; |
| 647 |
// } |
| 648 |
// } |
| 649 |
|
| 650 |
// pcBasis[yij] = Vector3.Cross(*pvBasis[yi], *pvBasis[xi]); |
| 651 |
// } |
| 652 |
|
| 653 |
// pvBasis[yi]->Normalize(); |
| 654 |
|
| 655 |
// if (pScale[zi] < 0.0001f) |
| 656 |
// *(pvBasis[zi]) = Vector3.Cross(*pvBasis[yi], *pvBasis[xi]); |
| 657 |
// else |
| 658 |
// pvBasis[zi]->Normalize(); |
| 659 |
|
| 660 |
// float rotDet = rotation.Determinant(); |
| 661 |
|
| 662 |
// if (rotDet < 0.0f) |
| 663 |
// { |
| 664 |
// pScale[xi] = -pScale[xi]; |
| 665 |
// *(pvBasis[xi]) = -(*(pvBasis[xi])); |
| 666 |
// rotDet = -rotDet; |
| 667 |
// } |
| 668 |
|
| 669 |
// outScale = scale; |
| 670 |
|
| 671 |
// if (Math.Abs(rotDet - 1.0f) > 0.01f) |
| 672 |
// { |
| 673 |
// outRotation = Quaternion.Identity; |
| 674 |
// return false; |
| 675 |
// } |
| 676 |
// else |
| 677 |
// { |
| 678 |
// outRotation = Quaternion.CreateFromRotationMatrix(rotation); |
| 679 |
// return true; |
| 680 |
// } |
| 681 |
//} |
| 682 |
} |
| 683 |
} |
