mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
109 lines
2.5 KiB
C++
109 lines
2.5 KiB
C++
// Bullet Continuous Collision Detection and Physics Library
|
|
// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
//
|
|
// Quat.cpp
|
|
//
|
|
// Copyright (c) 2006 Simon Hobbs
|
|
//
|
|
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
|
|
//
|
|
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
|
//
|
|
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
//
|
|
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
//
|
|
// 3. This notice may not be removed or altered from any source distribution.
|
|
|
|
#ifdef WIN32
|
|
#if _MSC_VER >= 1310
|
|
|
|
#include "Quat.h"
|
|
#include "Maths.h"
|
|
|
|
bool Quat::IsFinite() const
|
|
{
|
|
if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()) && _finite(GetW()))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
Quat::Quat(const Matrix33& m)
|
|
{
|
|
float tr, s, q[4];
|
|
int i, j, k;
|
|
int nxt[3] = {1, 2, 0};
|
|
float x, y, z, w;
|
|
|
|
// Check the diagonal
|
|
tr = m[0][0] + m[1][1] + m[2][2];
|
|
if (tr >= 0.0f)
|
|
{
|
|
// Diagonal is positive
|
|
s = ::Sqrt(tr + 1.0f);
|
|
w = s * 0.5f;
|
|
s = 0.5f / s;
|
|
x = (m[1][2] - m[2][1]) * s;
|
|
y = (m[2][0] - m[0][2]) * s;
|
|
z = (m[0][1] - m[1][0]) * s;
|
|
}
|
|
else
|
|
{
|
|
// Diagonal is negative
|
|
i = 0;
|
|
if (m[1][1] > m[0][0]) i = 1;
|
|
if (m[2][2] > m[i][i]) i = 2;
|
|
j = nxt[i];
|
|
k = nxt[j];
|
|
|
|
s = ::Sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
|
|
q[i] = s * 0.5f;
|
|
if (s != 0.0f) s = 0.5f / s;
|
|
|
|
q[3] = (m[j][k] - m[k][j]) * s;
|
|
q[j] = (m[i][j] + m[j][i]) * s;
|
|
q[k] = (m[i][k] + m[k][i]) * s;
|
|
|
|
x = q[0];
|
|
y = q[1];
|
|
z = q[2];
|
|
w = q[3];
|
|
}
|
|
|
|
*this = Quat(x, y, z, w);
|
|
}
|
|
|
|
const Quat Slerp(const Quat& a, const Quat& b, const Scalar& t)
|
|
{
|
|
Quat e;
|
|
Scalar cosom, t0, t1;
|
|
|
|
cosom = Dot(a, b);
|
|
|
|
if (cosom < Scalar::Consts::Zero)
|
|
{
|
|
cosom = -cosom;
|
|
e = -b;
|
|
}
|
|
else
|
|
e = b;
|
|
|
|
if (cosom < 0.9999f)
|
|
{
|
|
float omega = ::Acos(cosom);
|
|
Scalar rcpSinom = Rcp(Scalar(::Sin(omega)));
|
|
t0 = Scalar(::Sin((1.0f - (float)t) * omega)) * rcpSinom;
|
|
t1 = Scalar(::Sin((float)t * omega)) * rcpSinom;
|
|
}
|
|
else
|
|
{
|
|
t0 = Scalar(1.0f) - t;
|
|
t1 = t;
|
|
}
|
|
|
|
return a * t0 + e * t;
|
|
}
|
|
|
|
#endif
|
|
#endif //#ifdef WIN32
|