280 lines
9.7 KiB
C++
280 lines
9.7 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library Maya Plugin
|
|
Copyright (c) 2008 Walt Disney Studios
|
|
|
|
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.
|
|
|
|
Written by: Nicola Candussi <nicola@fluidinteractive.com>
|
|
*/
|
|
|
|
//quat.h
|
|
|
|
#ifndef MVL_QUAT_H
|
|
#define MVL_QUAT_H
|
|
|
|
#include <limits>
|
|
|
|
#include "vec.h"
|
|
#include "mat.h"
|
|
|
|
//quaternions are vectors of size 4
|
|
// it's assumed that the layout is in the form (w, (x, y, z)),
|
|
// so that the identity quaternion is (1, 0, 0, 0)
|
|
|
|
namespace mvl {
|
|
|
|
//quaternion conjugate
|
|
template<typename T>
|
|
inline vec<T, 4>
|
|
qconj(vec<T, 4> const& rhs) {
|
|
return vec<T, 4>(-rhs[0], rhs[1], rhs[2], rhs[3]);
|
|
}
|
|
|
|
//quaternion identity
|
|
template<typename T>
|
|
inline
|
|
vec<T, 4>
|
|
qidentity() {
|
|
return vec<T, 4>(1, 0, 0, 0);
|
|
}
|
|
|
|
//quaternion - quaternion product
|
|
template<typename T1, typename T2>
|
|
inline
|
|
vec<typename promote_traits<T1, T2>::value_type, 4>
|
|
qprod(vec<T1, 4> const& lhs, vec<T2, 4> const& rhs) {
|
|
typedef typename promote_traits<T1, T2>::value_type value_type;
|
|
return vec<value_type, 4>((lhs(0)*rhs(0)) - (lhs(1)*rhs(1)) - (lhs(2)*rhs(2)) - (lhs(3)*rhs(3)),
|
|
(lhs(0)*rhs(1)) + (lhs(1)*rhs(0)) + (lhs(2)*rhs(3)) - (lhs(3)*rhs(2)),
|
|
(lhs(0)*rhs(2)) - (lhs(1)*rhs(3)) + (lhs(2)*rhs(0)) + (lhs(3)*rhs(1)),
|
|
(lhs(0)*rhs(3)) + (lhs(1)*rhs(2)) - (lhs(2)*rhs(1)) + (lhs(3)*rhs(0)));
|
|
}
|
|
|
|
//quanternion - vector product (rotation)
|
|
template<typename T1, typename T2>
|
|
inline
|
|
vec<typename promote_traits<T1, T2>::value_type, 3>
|
|
qprod(vec<T1, 4> const& q, vec<T2, 3> const& v) {
|
|
typedef typename promote_traits<T1, T2>::value_type value_type;
|
|
vec<value_type, 4> tmp = qprod(qprod(q, vec<value_type, 4>(0, v[0], v[1], v[2])), qconj(q));
|
|
return vec<value_type, 3>(tmp[0], tmp[1], tmp[2]);
|
|
}
|
|
|
|
//spherical interpolation between q0 and q1
|
|
template <typename T1, typename T2, typename T3>
|
|
inline
|
|
vec<typename promote_traits<T1, T2>::value_type, 4>
|
|
qslerp(vec<T1, 4> const& q1, vec<T2, 4> const& q2, T3 t) {
|
|
typedef typename promote_traits<T1, T2>::value_type value_type;
|
|
value_type omega, cosom, sinom, scale0, scale1;
|
|
vec<value_type, 4> tmp;
|
|
|
|
cosom = dot(q1, q2);
|
|
|
|
if (cosom < static_cast<value_type>(0.0)) {
|
|
cosom = -cosom;
|
|
tmp = -q2;
|
|
} else {
|
|
tmp = q2;
|
|
}
|
|
|
|
if ((static_cast<value_type>(1.0) - cosom) > std::numeric_limits<value_type>::epsilon()) {
|
|
omega = (value_type) acos(cosom);
|
|
sinom = sin(omega);
|
|
scale0 = sin((static_cast<value_type>(1.0) - t) * omega) / sinom;
|
|
scale1 = sin(t * omega) / sinom;
|
|
} else {
|
|
scale0 = static_cast<value_type>(1.0) - t;
|
|
scale1 = t;
|
|
}
|
|
|
|
return scale0 * q1 + scale1 * tmp;
|
|
}
|
|
|
|
//init quaternion from axis-angle
|
|
template<typename T1, typename T2>
|
|
inline
|
|
vec<typename promote_traits<T1, T2>::value_type, 4>
|
|
q_from_axis_angle(vec<T1, 3> const& axis, T2 theta) {
|
|
typedef typename promote_traits<T1, T2>::value_type value_type;
|
|
value_type sin_theta = sin(static_cast<value_type>(static_cast<value_type>(0.5)) * theta);
|
|
return vec<value_type, 4>(cos(static_cast<value_type>(static_cast<value_type>(0.5)) * theta),
|
|
sin_theta * axis[0],
|
|
sin_theta * axis[1],
|
|
sin_theta * axis[2]);
|
|
}
|
|
|
|
//get the axis/angle from quaternion
|
|
template<typename T1, typename T2, typename T3>
|
|
inline
|
|
void
|
|
q_to_axis_angle(vec<T1, 4> const& q, vec<T2, 3>& axis, T3& theta)
|
|
{
|
|
T3 half_theta= acos(q[0]);
|
|
|
|
if(half_theta > 10 * std::numeric_limits<T3>::epsilon()) {
|
|
T3 oost = 1 / sin(half_theta);
|
|
|
|
axis[0] = oost * q[1];
|
|
axis[1] = oost * q[2];
|
|
axis[2] = oost * q[3];
|
|
theta = 2 * half_theta;
|
|
} else {
|
|
axis[0] = axis[1] = axis[2] = 0;
|
|
theta = 0;
|
|
}
|
|
}
|
|
|
|
//init quaternion from rotation matrix
|
|
template<typename T1>
|
|
inline
|
|
vec<T1, 4>
|
|
q_from_mat(mat<T1, 3, 3> const& m) {
|
|
T1 trace, s, hos;
|
|
trace = m(0, 0) + m(1, 1) + m(2, 2);
|
|
if (trace > static_cast<T1>(0.0)) {
|
|
s = sqrt(trace + static_cast<T1>(1.0));
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>(s * static_cast<T1>(0.5), (m(2, 1) - m(1, 2)) * hos, (m(0, 2) - m(2, 0)) * hos, (m(1, 0) - m(0, 1)) * hos);
|
|
} else {
|
|
int biggest;
|
|
enum {A,T,I};
|
|
if (m(0, 0) > m(1, 1)) {
|
|
if (m(2, 2) > m(0, 0)) biggest = I;
|
|
else biggest = A;
|
|
} else {
|
|
if (m(2, 2) > m(0, 0)) biggest = I;
|
|
else biggest = T;
|
|
}
|
|
switch (biggest) {
|
|
case A:
|
|
s = sqrt( m(0, 0) - (m(1, 1) + m(2, 2)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(2, 1) - m(1, 2)) * hos, s * static_cast<T1>(0.5), (m(0, 1) + m(1, 0)) * hos, (m(0, 2) + m(2, 0)) * hos);
|
|
}
|
|
// I
|
|
s = sqrt( m(2, 2) - (m(0, 0) + m(1, 1)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(1, 0) - m(0, 1)) * hos, (m(2, 0) + m(0, 2)) * hos, (m(2, 1) + m(1, 2)) * hos, s * static_cast<T1>(0.5));
|
|
}
|
|
// T
|
|
s = sqrt( m(1, 1) - (m(2, 2) + m(0, 0)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(0, 2) - m(2, 0)) * hos, (m(1, 0) + m(0, 1)) * hos, s * static_cast<T1>(0.5), (m(1, 2) + m(2, 1)) * hos);
|
|
}
|
|
break;
|
|
case T:
|
|
s = sqrt( m(1, 1) - (m(2, 2) + m(0, 0)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(0, 2) - m(2, 0)) * hos, (m(1, 0) + m(0, 1)) * hos, s * static_cast<T1>(0.5), (m(1, 2) + m(2, 1)) * hos);
|
|
}
|
|
// I
|
|
s = sqrt( m(2, 2) - (m(0, 0) + m(1, 1)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(1, 0) - m(0, 1)) * hos, (m(2, 0) + m(0, 2)) * hos, (m(2, 1) + m(1, 2)) * hos, s * static_cast<T1>(0.5));
|
|
}
|
|
// A
|
|
s = sqrt( m(0, 0) - (m(1, 1) + m(2, 2)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(2, 1) - m(1, 2)) * hos, s * static_cast<T1>(0.5), (m(0, 1) + m(1, 0)) * hos, (m(0, 2) + m(2, 0)) * hos);
|
|
}
|
|
break;
|
|
case I:
|
|
s = sqrt( m(2, 2) - (m(0, 0) + m(1, 1)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(1, 0) - m(0, 1)) * hos, (m(2, 0) + m(0, 2)) * hos, (m(2, 1) + m(1, 2)) * hos, s * static_cast<T1>(0.5));
|
|
}
|
|
// A
|
|
s = sqrt( m(0, 0) - (m(1, 1) + m(2, 2)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(2, 1) - m(1, 2)) * hos, s * static_cast<T1>(0.5), (m(0, 1) + m(1, 0)) * hos, (m(0, 2) + m(2, 0)) * hos);
|
|
}
|
|
// T
|
|
s = sqrt( m(1, 1) - (m(2, 2) + m(0, 0)) + static_cast<T1>(1.0));
|
|
if (s > (100 * std::numeric_limits<T1>::epsilon())) {
|
|
hos = static_cast<T1>(0.5) / s;
|
|
return vec<T1, 4>((m(0, 2) - m(2, 0)) * hos, (m(1, 0) + m(0, 1)) * hos, s * static_cast<T1>(0.5), (m(1, 2) + m(2, 1)) * hos);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
//get rotation matrix from quaternion
|
|
template<typename T>
|
|
inline
|
|
void
|
|
q_to_mat(vec<T, 4> const& q, mat<T, 3, 3>& m) {
|
|
T X2,Y2,Z2; //2*QX, 2*QY, 2*QZ
|
|
T XX2,YY2,ZZ2; //2*QX*QX, 2*QY*QY, 2*QZ*QZ
|
|
T XY2,XZ2,XW2; //2*QX*QY, 2*QX*QZ, 2*QX*QW
|
|
T YZ2,YW2,ZW2; // ...
|
|
|
|
X2 = 2 * q[1];
|
|
XX2 = X2 * q[1];
|
|
XY2 = X2 * q[2];
|
|
XZ2 = X2 * q[3];
|
|
XW2 = X2 * q[0];
|
|
|
|
Y2 = 2 * q[2];
|
|
YY2 = Y2 * q[2];
|
|
YZ2 = Y2 * q[3];
|
|
YW2 = Y2 * q[0];
|
|
|
|
Z2 = 2 * q[3];
|
|
ZZ2 = Z2 * q[3];
|
|
ZW2 = Z2 * q[0];
|
|
|
|
m(0, 0) = 1 - YY2 - ZZ2;
|
|
m(0, 1) = XY2 - ZW2;
|
|
m(0, 2) = XZ2 + YW2;
|
|
|
|
m(1, 0) = XY2 + ZW2;
|
|
m(1, 1) = 1 - XX2 - ZZ2;
|
|
m(1, 2) = YZ2 - XW2;
|
|
|
|
m(2, 0) = XZ2 - YW2;
|
|
m(2, 1) = YZ2 + XW2;
|
|
m(2, 2) = 1 - XX2 - YY2;
|
|
|
|
}
|
|
|
|
template<typename T, size_t R, size_t C>
|
|
mat<T, R, C>
|
|
cmat(T const* m)
|
|
{
|
|
mat<T, R, C> res;
|
|
for(int i = 0; i < R; ++i) {
|
|
for(int j = 0; j < C; ++j) {
|
|
res(i, j) = m[i * C + j];
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
} //namespace mvl
|
|
|
|
|
|
#endif
|