///grabbed
void parent(CommonRigidBodyBase* bullet_scene, int id, quaternion base, quaternion current_parent, vec3f position_offset, vec3f ideal_position_offset, bool slide = false)
{
parent_id = id;
base_diff = current_parent.get_difference(base);
should_hand_collide = false;
make_kinematic(bullet_scene);
mat3f hand_rot = current_parent.get_rotation_matrix();
offset = hand_rot.transp() * position_offset;
ideal_offset = hand_rot.transp() * ideal_position_offset;
is_parented = true;
if(slide && can_slide)
{
slide_towards_parent = true;
slide_timer = 0;
slide_parent_init = false;
}
//printf("%f %f %f %f base\n", base_diff.x(), base_diff.y(), base_diff.z(), base_diff.w());
}
quaternion exp( const quaternion& lhs ){
float alpha = lhs.norm();
vec3 v = normalize3( lhs.comps().xyz() );
float sin_alpha = 0.0f;
float cos_alpha = 0.0f;
sincos(alpha, sin_alpha, cos_alpha);
vec4 q_v;
q_v.w( cos_alpha );
q_v.xyz( sin_alpha * v );
return quaternion(q_v);
}
void quaternion::Intermediate(const quaternion &rkQ0, const quaternion &rkQ1, const quaternion &rkQ2, quaternion &rkA, quaternion &rkB)
{
// assert: q0, q1, q2 are unit quaternions
quaternion kQ0inv = rkQ0.UnitInverse();
quaternion kQ1inv = rkQ1.UnitInverse();
quaternion rkP0 = kQ0inv*rkQ1;
quaternion rkP1 = kQ1inv*rkQ2;
quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
quaternion kMinusArg = -kArg;
rkA = rkQ1*kArg.Exp();
rkB = rkQ1*kMinusArg.Exp();
}
// Uses the given angular velocity and time interval to calculate
// a rotation and applies that rotation to the given quaternion.
// w is angular velocity in radians per second.
// dt is the time.
void ahrs::rotate(quaternion& rotation, const vector& w, float dt)
{
// Multiply by first order approximation of the
// quaternion representing this rotation.
rotation *= quaternion(1, w(0)*dt/2, w(1)*dt/2, w(2)*dt/2);
rotation.normalize();
}
Foam::triad::triad(const quaternion& q)
{
tensor Rt(q.R().T());
x() = Rt.x();
y() = Rt.y();
z() = Rt.z();
}
quaternion slerp(const quaternion& p1, const quaternion& q1, float t) {
quaternion q = q1.normalise();
quaternion p = p1.normalise();
float epsilon = 0.0001;
if (dotproduct(p, q) < 0) {
q = q * -1;
}
float dpq = dotproduct(p, q);
if ((1.0 - dpq) > epsilon) {
float w = acos(dpq);
return ((sin((1 - t) * w) * p) + (sin(t * w) * q)) / sin(w);
} else {
return (1 - t) * p + t * q;
}
}
void init(objects_container* _ctr, btRigidBody* _rigid_body, bool _can_slide = false)
{
ctr = _ctr;
rigid_body = _rigid_body;
//if(ctr)
// b = get_bbox(ctr);
//else
{
btVector3 bmin;
btVector3 bmax;
btTransform none;
none.setOrigin(btVector3(0,0,0));
none.setRotation(btQuaternion().getIdentity());
rigid_body->getCollisionShape()->getAabb(none, bmin, bmax);
b.min = {bmin.x(), bmin.y(), bmin.z()};
b.max = {bmax.x(), bmax.y(), bmax.z()};
}
can_slide = _can_slide;
base_diff = base_diff.identity();
}
void ahrs::fuse_default(quaternion& rotation, float dt, const vector& angular_velocity,
const vector& acceleration, const vector& magnetic_field)
{
vector correction = vector(0, 0, 0);
if (abs(acceleration.norm() - 1) <= 0.3)
{
// The magnitude of acceleration is close to 1 g, so
// it might be pointing up and we can do drift correction.
const float correction_strength = 1;
matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);
matrix rotationMatrix = rotation.toRotationMatrix();
correction = (
rotationCompass.row(0).cross(rotationMatrix.row(0)) +
rotationCompass.row(1).cross(rotationMatrix.row(1)) +
rotationCompass.row(2).cross(rotationMatrix.row(2))
) * correction_strength;
}
rotate(rotation, angular_velocity + correction, dt);
}
void clipmap_ring::update(const vector & eye_dir_in_object_space)
{
// get quaternion rotations for clipmap
double view_phi = ::acos(eye_dir_in_object_space[2]);
double view_theta = ::atan2(eye_dir_in_object_space[1], eye_dir_in_object_space[0]);
if ( level && !( (::fabs(view_phi - old_view_phi) > VIEW_EPSILON) || (::fabs(view_theta - old_view_theta) > VIEW_EPSILON) ) )
{
return;
}
old_view_phi = view_phi;
old_view_theta = view_theta;
roty = quaternion(vector::Y_AXIS, view_phi);
roty_conj = roty.conjugate();
rotz = quaternion(vector::Z_AXIS, view_theta);
rotz_conj = rotz.conjugate();
// generate vertices & indices
generate_vertices();
generate_indices();
// create buffers if necessary
if (!vbuf_id)
{
glGenBuffers(1, (GLuint *) &vbuf_id);
if (!vbuf_id)
throw opengl_exception(__FILE__, __LINE__, L"clipmap_ring unable to generate buffer id: ");
}
if (!ibuf_id)
{
glGenBuffers(1, (GLuint *) &ibuf_id);
if (!ibuf_id)
throw opengl_exception(__FILE__, __LINE__, L"clipmap_ring unable to generate buffer id");
}
// update buffers
glBindBuffer(GL_ARRAY_BUFFER, vbuf_id);
glBufferData(GL_ARRAY_BUFFER, sizeof(float) * vbuf.size(), vbuf.ptr(), GL_STREAM_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ibuf_id);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(unsigned int) * ibuf.size(), ibuf.ptr(), GL_STREAM_DRAW);
} // clipmap_ring::update()
quaternion log( const quaternion& lhs ){
float alpha = acos(lhs.w);
if( equal( std::abs(alpha), 1.0f ) ){
return lhs;
}
vec3 new_v = normalize3( lhs.comps().xyz() );
return quaternion(alpha*new_v[0], alpha*new_v[1], alpha*new_v[2], 0.0f);
}
void Foam::transform
(
vectorField& rtf,
const quaternion& q,
const vectorField& tf
)
{
tensor t = q.R();
TFOR_ALL_F_OP_FUNC_S_F(vector, rtf, =, transform, tensor, t, vector, tf)
}
quaternion slerp( const quaternion& src, const quaternion& dest, float t )
{
float cos_omega = dot_prod4(src.comps(), dest.comps());
vec4 near_dest_v = dest.comps() * sign(cos_omega);
cos_omega = abs(cos_omega);
float k0(0.0f), k1(0.0f);
if( equal(cos_omega, 1.0f) ){
k0 = 1.0f - t;
k1 = t;
}else{
float sin_omega = sqrt(1.0f - cos_omega*cos_omega);
float omega = atan2(sin_omega, cos_omega);
float inv_sin_omega = 1.0f / sin_omega;
k0 = sin( (1.0f - t) * omega ) * inv_sin_omega;
k1 = sin( t * omega ) * inv_sin_omega;
}
return quaternion( src.comps() * k0 + near_dest_v * k1);
}
void set_trans(cl_float4 clpos, quaternion m)
{
mat3f mat_diff = base_diff.get_rotation_matrix();
mat3f current_hand = m.get_rotation_matrix();
mat3f my_rot = current_hand * mat_diff;
quaternion n;
n.load_from_matrix(my_rot);
vec3f absolute_pos = {clpos.x, clpos.y, clpos.z};
///current hand does not take into account the rotation offset when grabbing
///ie we'll double rotate
vec3f offset_rot = current_hand * offset;
vec3f pos = absolute_pos + offset_rot;
btTransform newTrans;
//rigid_body->getMotionState()->getWorldTransform(newTrans);
newTrans.setOrigin(btVector3(pos.v[0], pos.v[1], pos.v[2]));
newTrans.setRotation(btQuaternion(n.x(), n.y(), n.z(), n.w()));
rigid_body->getMotionState()->setWorldTransform(newTrans);
//rigid_body->setInterpolationWorldTransform(newTrans);
//if(ctr)
// ctr->set_pos(conv_implicit<cl_float4>(pos));
slide_parent_init = true;
slide_saved_parent = absolute_pos;
remote_pos = pos;
remote_rot = n;
kinematic_old = kinematic_current;
kinematic_current = xyzf_to_vec(rigid_body->getWorldTransform().getOrigin());
}
quaternion quaternion::nlerp(real fT, const quaternion &rkP, const quaternion &rkQ, bool shortestPath)
{
quaternion result;
real fCos = rkP.Dot(rkQ);
if (fCos < 0.0f && shortestPath)
{
result = rkP + fT * ((-rkQ) - rkP);
}
else
{
result = rkP + fT * (rkQ - rkP);
}
result.normalise();
return result;
}
quaternion quaternion::SlerpExtraSpins(real fT, const quaternion& rkP, const quaternion& rkQ, int iExtraSpins)
{
real fCos = rkP.Dot(rkQ);
real fAngle (math::acos(fCos) );
if (math::abs(fAngle) < msEpsilon )
return rkP;
real fSin = math::sin(fAngle);
real fPhase ( math::pi*iExtraSpins*fT );
real fInvSin = 1.0f/fSin;
real fCoeff0 = math::sin((1.0f-fT)*fAngle - fPhase)*fInvSin;
real fCoeff1 = math::sin(fT*fAngle + fPhase)*fInvSin;
return fCoeff0*rkP + fCoeff1*rkQ;
}
quaternion quaternion::Slerp(real fT, const quaternion &rkP, const quaternion &rkQ, bool shortestPath)
{
real fCos = rkP.Dot(rkQ);
quaternion rkT;
// Do we need to invert rotation?
if (fCos < 0.0f && shortestPath)
{
fCos = -fCos;
rkT = -rkQ;
}
else
{
rkT = rkQ;
}
if (math::abs(fCos) < 1 - msEpsilon)
{
// Standard case (slerp)
real fSin = math::sqrt(1 - fCos*fCos);
real fAngle = math::atan2(fSin, fCos);
real fInvSin = 1.0f / fSin;
real fCoeff0 = math::sin((1.0f - fT) * fAngle) * fInvSin;
real fCoeff1 = math::sin(fT * fAngle) * fInvSin;
return fCoeff0 * rkP + fCoeff1 * rkT;
}
else
{
// There are two situations:
// 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
// interpolation safely.
// 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
// are an infinite number of possibilities interpolation. but we haven't
// have method to fix this case, so just use linear interpolation here.
quaternion t = (1.0f - fT) * rkP + fT * rkT;
// taking the complement requires renormalisation
t.normalise();
return t;
}
}
void matrix4x4::setOrientation( quaternion in )
{
jAssert( fcmp( in.length(), 1 ) );
JFLOAT xx = in.x() * in.x();
JFLOAT xy = in.x() * in.y();
JFLOAT xz = in.x() * in.z();
JFLOAT xw = in.x() * in.w();
JFLOAT yy = in.y() * in.y();
JFLOAT yz = in.y() * in.z();
JFLOAT yw = in.y() * in.w();
JFLOAT zz = in.z() * in.z();
JFLOAT zw = in.z() * in.w();
#ifndef USEALTERNATIVEQUATTOMATRIX
JFLOAT ww = in.w() * in.w();
RM(0,0) = xx - yy - zz + ww;
RM(1,0) = 2 * ( xy - zw );
RM(2,0) = 2 * ( xz + yw );
RM(0,1) = 2 * ( xy + zw );
RM(1,1) = -xx + yy - zz + ww;
RM(2,1) = 2 * ( yz - xw );
RM(0,2) = 2 * ( xz - yw );
RM(1,2) = 2 * ( yz + xw );
RM(2,2) = -xx - yy + zz + ww;
#else
RM(0,0) = 1.0 - 2.0 * ( yy + zz );
RM(0,1) = 2 * ( xy + zw );
RM(0,2) = 2 * ( xz - yw );
RM(1,0) = 2 * ( xy - zw );
RM(1,1) = 1 - 2 * ( xx + zz );
RM(1,2) = 2 * ( yz + xw );
RM(2,0) = 2 * ( xz + yw );
RM(2,1) = 2 * ( yz - xw );
RM(2,2) = 1 - 2 * ( xx + yy );
#endif
}
void Camera::rotate(const quaternion& q)
{
_modelViewMatrix *= q.toMatrix();
modelViewUpdated();
}
quaternion(quaternion const& other):container(other.data()){}
quaternion operator-(quaternion const& q)
{
return quaternion(-q.x(),-q.y(),-q.z(),-q.w());
}
quaternion operator*(quaternion const& lhs,quaternion const& rhs)
{
return quaternion(lhs.x()*rhs.w() + lhs.w()*rhs.x() + lhs.y()*rhs.z() - lhs.z()*rhs.y(),
lhs.y()*rhs.w() + lhs.w()*rhs.y() + lhs.z()*rhs.x() - lhs.x()*rhs.z(),
lhs.z()*rhs.w() + lhs.w()*rhs.z() + lhs.x()*rhs.y() - lhs.y()*rhs.x(),
lhs.w()*rhs.w() - lhs.x()*rhs.x() - lhs.y()*rhs.y() - lhs.z()*rhs.z());
}
void quaternion::multiply(quaternion q) {
float w_prime = w * q.get_w() - x * q.get_x() - y * q.get_y()
- z * q.get_z();
float x_prime = x * q.get_w() + w * q.get_x() + y * q.get_z()
- z * q.get_y();
float y_prime = y * q.get_w() + w * q.get_y() + z * q.get_x()
- x * q.get_z();
float z_prime = z * q.get_w() + w * q.get_z() + x * q.get_y()
- y * q.get_x();
this->x = x_prime;
this->y = y_prime;
this->z = z_prime;
this->w = w_prime;
}
float dot(quaternion const& q0,quaternion const& q1)
{
return q0.x()*q1.x()+q0.y()*q1.y()+q0.z()*q1.z()+q0.w()*q1.w();
}
void matrix3::to_quaternion(quaternion &out_q) const {
out_q.from_matrix((*this));
}
void setRotate(const quaternion<Type>& a_quat)
{
*this = a_quat.getMatrix();
}
vector3df QuaternionToEuler(const quaternion& quat)
{
vector3df eulerRot;
quat.toEuler(eulerRot);
return eulerRot;
}
int main()
{
array<double, 3> axis;
axis[0] = 1;
axis[1] = 0;
axis[2] = 0;
quaternion<double>
q = quaternion_from_rotation(axis, 90 * deg2rad),
qq = quaternion_from_rotation(axis, 172 * deg2rad);
{
const quaternion<double> conj(q.conjugate());
assert((q + conj) == (q.scalar() * 2));
{
const quaternion<double> tmp(q - conj);
assert(tmp.scalar() == 0);
assert(tmp.vector() == (q.vector() * 2));
}
assert(q.norm() == conj.norm());
CLOSE_ENOUGH((q * qq).norm(), q.norm() * qq.norm());
assert(q == q.inverse().inverse());
{
quaternion<double> r(q * qq);
// assert((r * qq.inverse()) == q);
// assert((q.inverse() * r) == qq);
}
}
axis[0] = 1;
axis[1] = 0;
axis[2] = 0;
quaternion<double> rx = quaternion_from_rotation(axis, 90 * deg2rad);
axis[0] = 0;
axis[1] = 1;
axis[2] = 0;
quaternion<double> ry = quaternion_from_rotation(axis, 90 * deg2rad);
axis[0] = 0;
axis[1] = 0;
axis[2] = 1;
quaternion<double> rz = quaternion_from_rotation(axis, 90 * deg2rad);
array<double, 3> vector;
vector[0] = 1;
vector[1] = 0;
vector[2] = 0;
std::cout
<< "Orginal vector: " << vector << std::endl
<< "Rotated around x: " << rotate_with_quaternion(vector, rx) << std::endl
<< "Rotated around y: " << rotate_with_quaternion(vector, ry) << std::endl
<< "Rotated around z: " << rotate_with_quaternion(vector, rz) << std::endl
<< "Rotated around x and y: " << rotate_with_quaternion(vector, ry * rx) << std::endl
<< "Rotated around x, y and z: " << rotate_with_quaternion(vector, rz * ry * rx) << std::endl;
return EXIT_SUCCESS;
}
quaternion operator/(const quaternion& q1, const quaternion& q2) {
quaternion q2inv = q2.multiplicativeInverse();
quaternion r = q1 * q2inv;
return r;
}
void ahrs::output_euler(quaternion & rotation)
{
std::cout << (vector)(rotation.toRotationMatrix().eulerAngles(2, 1, 0) * (180 / M_PI));
}
void ahrs::output_matrix(quaternion & rotation)
{
std::cout << rotation.toRotationMatrix();
}
